A pipeline robot positioning method, system, device and medium based on a mile wheel and an IMU

Abstract

The application discloses a pipeline robot positioning method, system and device based on a mileage 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 a pipeline robot in a static state according to acceleration data and magnetic force data collected by an IMU; updating the initial attitude angle by using an Euler angle method to obtain an attitude angle matrix based on a compensated angular velocity obtained by denoising angular velocity data collected by the IMU by using a complementary filtering algorithm, and performing optimal estimation by using a nonlinear state estimation algorithm; performing consistency checking on pulse data of multiple mileage wheels, determining corresponding effective pulse data by weighted correction, and then determining a mileage increment; and projecting the mileage increment to a navigation coordinate system based on the optimized attitude angle matrix to solve real-time position coordinates of the pipeline robot. The application can solve the problems that the pipeline robot has poor passability and low positioning accuracy in a complex pipeline network.

Description

TECHNICAL FIELD

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

[0002] With the development of urbanization, the scale of urban underground pipe network is expanding, and the role of pipeline robots in pipe network detection and maintenance is becoming increasingly important. However, the underground pipeline environment generally has complex characteristics such as the absence of global satellite navigation system (GNSS) signals, limited internal space, wet and slippery pipe walls, and many sediments, which pose a serious challenge to the accurate positioning of robots.

[0003] Existing pipeline positioning technologies have significant shortcomings when dealing with complex pipe network structures. In particular, when encountering complex working conditions such as right-angle bends, multi-branch pipes (such as three-way and four-way pipes), variable-diameter pipes, or inclined pipes, traditional positioning devices are often bulky and lack flexibility, making it difficult to adapt to changes in different pipe diameters and directions, resulting in severe limitations on passability and the inability to complete comprehensive mapping. In addition, the driving method relying on external traction further limits the autonomous operation capability of pipeline robots in complex pipe networks.

[0004] In terms of positioning sensors, error accumulation is a particular problem. The measurement method based on the odometer is prone to slipping when the odometer is in water, covered with sediments, or driving on a curve, resulting in displacement measurement deviation, and this deviation continues to accumulate over time. The inertial measurement unit (IMU) can provide real-time attitude information, but its inherent sensor zero offset drift characteristic will cause the positioning error to increase nonlinearly over a long period of time. Relying solely on the odometer or the IMU cannot meet the engineering requirements of high precision and long time positioning in complex and narrow pipe environments. Therefore, there is an urgent need to develop new positioning solutions to solve the problem of accurate positioning of robots in complex pipe networks. SUMMARY

[0005] To solve the problems in the prior art, the present application provides a pipeline robot positioning method and system based on an odometer and an IMU, which can solve the problems of poor passability and low positioning accuracy of pipeline robots in complex pipe networks.

[0006] To achieve the above-mentioned purpose, the present application provides a pipeline robot positioning method based on an odometer and an IMU; the pipeline robot comprises a first section body, a second section body, a third section body and a fourth section body connected in series, and odometers are arranged on both sides of each hinge; the method comprises:

[0007] determining the initial attitude angle of the pipeline robot in the static state according to the acceleration data and magnetic force data collected by the IMU;

[0008] The complementary filtering algorithm is used to denoise the angular velocity data collected by the IMU to obtain compensated angular velocity;

[0009] Based on the compensated angular velocity, the Euler angle method is used to update the initial attitude angle to obtain an attitude angle matrix;

[0010] The nonlinear state estimation algorithm is used to optimally estimate the attitude angle matrix to obtain an optimized attitude angle matrix;

[0011] The pulse data of multiple odometer wheels are consistency checked, and the corresponding effective pulse data is determined through weighted correction;

[0012] According to the effective pulse data, the mileage increment is determined;

[0013] 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.

[0014] Optionally, the ends of the first and fourth segments are each provided with a passive wheel mechanism, and the first and second segments and the third and fourth segments are each provided with an elastic component; the pulse data of multiple odometer wheels are consistency checked, and the corresponding effective pulse data is determined through weighted correction, including:

[0015] The maximum difference value between each two pulse data is determined;

[0016] If the maximum difference value is less than or equal to a preset threshold, the average value of all pulse data is taken as the effective pulse data;

[0017] If the maximum difference value is greater than the preset threshold, the rotation signals of each passive wheel mechanism and the tension feedback data of each elastic component are obtained;

[0018] According to the rotation signals, the actual motion direction of the pipeline robot is determined, and the slipping odometer wheel is identified in combination with the tension feedback data;

[0019] Based on the tension feedback data, the pulse data corresponding to the slipping odometer wheel is weighted and corrected to generate effective pulse data.

[0020] Optionally, the initial attitude angle includes an initial pitch angle, an initial roll angle and an initial heading angle; according to the acceleration data and magnetic force data collected by the IMU, the initial attitude angle of the pipeline robot in the static state is determined, including:

[0021] According to the gravity component in the inertial navigation coordinate system acquired through the acceleration data of the IMU, the initial pitch angle and the initial roll angle of the pipeline robot in the carrier coordinate system in the static state are solved.

[0022] According to the local magnetic declination and the magnetic force data acquired through the IMU, the initial heading angle is calculated.

[0023] Optionally, the IMU is fixedly installed on the third section body and is arranged to deviate from the pipeline robot axis.

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

[0025] The application further provides a pipeline robot positioning system based on a mileage wheel and an IMU, the pipeline robot comprising a first section body, a second section body, a third section body and a fourth section body which are sequentially and serially connected and hinged, and mileage wheels are arranged on both sides of each hinge; the system comprises:

[0026] a posture angle determination unit, configured to:

[0027] determine the initial posture angle of the pipeline robot in the static state according to the acceleration data and the magnetic force data acquired through the IMU;

[0028] compensate the angular velocity data acquired by the IMU by using a complementary filter algorithm to obtain compensated angular velocity;

[0029] update the initial posture angle by using an Euler angle method based on the compensated angular velocity to obtain a posture angle matrix;

[0030] perform optimal estimation on the posture angle matrix by using a nonlinear state estimation algorithm to obtain an optimized posture angle matrix;

[0031] a mileage determination unit, configured to:

[0032] perform consistency check on the pulse data of the plurality of mileage wheels and determine corresponding effective pulse data through weighted correction;

[0033] determine a mileage increment according to the effective pulse data;

[0034] a position determination unit, configured to project the mileage increment from the carrier coordinate system to a navigation coordinate system based on the optimized posture angle matrix to solve the real-time position coordinates of the pipeline robot.

[0035] The application further provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to implement the positioning method.

[0036] According to the specific embodiments of the present application, the following technical effects are disclosed:

[0037] The pipeline robot positioning method based on the odometer and the IMU provided by the present application significantly improves the passability of the pipeline robot in the complex pipeline network such as the variable-diameter pipeline, the right-angle elbow pipeline and the multi-pass pipeline, and solves the problem of limited passability caused by the large size and poor flexibility of the traditional positioning device.

[0038] The initial alignment of the IMU determines the static attitude angle, the gyro angular velocity data is denoised and the drift is suppressed by the complementary filtering, and then the attitude matrix is updated by the Euler angle method and optimized by the extended Kalman filtering, so that the long-time attitude error nonlinear accumulation problem caused by the zero drift of the IMU is effectively overcome, and the attitude angle measurement accuracy is significantly improved. In view of the displacement error accumulation caused by the slippage of the odometer in the water, the sediment or the curve, the slippage wheel set is dynamically identified and the data weight thereof is reduced through the consistency verification and the weighted correction mechanism of the multi-odometer pulse data, and the effective pulse data is fused to greatly suppress the displacement measurement deviation caused by the slippage of the single odometer.

[0039] Finally, the optimized attitude angle matrix is used to project the odometer increment from the carrier coordinate system to the navigation coordinate system to solve the position, and the advantages of the high dynamic attitude data of the IMU and the displacement data of the odometer are fused, so that the high-precision dead reckoning can be realized even in the pipeline environment without GNSS signal. The present application not only solves the defect of insufficient positioning reliability of a single sensor, but also realizes the long-time and high-precision autonomous positioning in the complex and narrow pipeline environment through the collaborative optimization of multiple data sources, and provides reliable technical support for the pipeline detection and maintenance. BRIEF DESCRIPTION OF DRAWINGS

[0040] The above and other objects, features and advantages of the present application will become more apparent from the following detailed description when taken in conjunction with the accompanying drawings in which like reference characters refer to like parts throughout the figures, and wherein exemplary embodiments of the present application are shown.

[0041] Figure 1 A method flowchart of the pipeline robot positioning method based on the odometer and the IMU according to the embodiments of the present application is shown;

[0042] Figure 2 A scene diagram of the pipeline robot passing through various complex pipeline conditions according to the embodiments of the present application is shown;

[0043] Figure 3 A structure flowchart of the pipeline robot positioning method according to the embodiments of the present application is shown;

[0044] Figure 4A schematic view of the contact between the odometer and the pipe wall during the pipeline positioning surveying and mapping of the embodiment of the present application is shown.

[0045] Figure 5 A schematic view of the data fusion positioning algorithm structure of the embodiment of the present application is shown.

[0046] Figure 6 A schematic view of the pipeline robot positioning method flow of the embodiment of the present application is shown.

[0047] Figure 7 A schematic view of the definition of each coordinate system in the pipeline robot positioning algorithm of the embodiment of the present application is shown.

[0048] Figure 8 A schematic view of the module structure of the pipeline robot positioning system of the embodiment of the present application is shown.

[0049] Figure 9 A schematic view of the structure of the electronic device of the embodiment of the present application is shown. DETAILED DESCRIPTION

[0050] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0051] Please refer to Figure 1 , Figure 1 A schematic view of the method flow of the pipeline robot positioning method based on the odometer and the IMU is shown.

[0052] The pipeline robot provided by the present application comprises a plurality of active serial joint body units, and the connection between adjacent joint body units is provided with an odometer. Specifically, the pipeline robot comprises a first joint body, a second joint body, a third joint body and a fourth joint body which are sequentially and hingedly connected, and an odometer is arranged on both sides of each hinge. Please refer to Figure 2 , Figure 2 The pipeline robot of the present application passes through various complex pipe network scenes. The pipeline robot can pass through complex pipe conditions such as variable diameter pipes, elbow pipes, three-way pipes and four-way pipes, and has strong passing ability and can be used for positioning and mapping of complex pipe networks.

[0053] The pipeline robot positioning method comprises:

[0054] S101: determining an initial attitude angle of the pipeline robot in a static state according to acceleration data and magnetic force data collected by the IMU.

[0055] The initial attitude angle includes an initial pitch angle, an initial roll angle, and an initial heading angle. Before the pipeline robot performs a positioning task, it needs to be ensured that it is in a static state to perform accurate initial attitude alignment. At this time, the micro-electromechanical system (MEMS) and the inertial measurement unit (IMU) installed on the robot joint unit and deviating from the robot axis start to work. The IMU includes a three-axis accelerometer and a three-axis magnetometer; the IMU is fixedly installed on the joint unit deviating from the axis of the pipeline robot, for example, can be fixedly installed on the third joint.

[0056] The initial attitude angle of the pipeline robot in the static state is determined according to the acceleration data and the magnetic force data collected by the IMU, and specifically includes:

[0057] The initial pitch angle and the initial roll angle of the pipeline robot in the static state in the carrier coordinate system are calculated according to the gravity component of the acceleration data collected by the IMU in the inertial navigation coordinate system.

[0058] The initial heading angle is calculated according to the local magnetic declination and the magnetic force data collected by the IMU.

[0059] Specifically, first, the acceleration data of the robot in the static state is collected by the three-axis accelerometer of the IMU. In the static state, the robot is only subjected to the action of gravity, and the theoretical output of the accelerometer in the inertial navigation 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 angle and the initial roll angle of the pipeline robot in the current pipeline space can be calculated. Specifically, by calculating the relationship between the measured value in the inertial navigation coordinate system and the theoretical gravity vector in the carrier coordinate system, the inverse trigonometric function can be used to solve the two attitude angles, which reflect the front and rear inclination and the left and right inclination angles of the robot relative to the horizontal plane.

[0060] Secondly, the magnetic force data (environmental magnetic field strength vector) in the static state is collected by the three-axis magnetometer of the IMU. Combined with the known local magnetic declination (i.e. the angle difference between the magnetic north direction and the geographic true north direction), the magnetic force data can be used to calculate the initial heading angle of the pipeline robot. The magnetometer measurement value provides the geomagnetic field vector information in the carrier coordinate system, which can be converted to the horizontal plane and compared with the geographic north reference (considering the magnetic declination correction) to calculate the initial heading angle , which represents the orientation of the head of the robot relative to the geographic north.

[0061] In addition, in order to obtain more reliable initial heading in complex underground pipeline environment (e.g. strong electromagnetic interference or metal structure), a fusion mechanism can be further introduced. Specifically, a high-precision MEMS inertial north module (through sensitive Coriolis force caused by earth rotation or geomagnetic field gradient change) can directly output a heading angle relative to true north . The final initial heading angle is obtained by weighted fusion and : ; wherein is the heading angle proportionality coefficient. In application, the value of can be automatically switched or adjusted according to the electromagnetic interference of the pipeline environment. When the interference is weak, the result of the magnetometer is mainly relied on (the is small); in strong interference or metal-intensive area, the weight of the high-precision north module result is increased (the is larger).

[0062] For example, the strength of electromagnetic interference can be determined by monitoring the fluctuation characteristics of the magnetometer measurement value in real time. When the variance of the magnetic data in the continuous three sampling periods exceeds the preset value (such as 10 μT), it is determined that it is a strong electromagnetic interference environment; at the same time, combined with the pre-set pipeline material database (including the characteristic parameters of conductive materials such as cast iron and stainless steel), or according to the pipeline environment image collected by the camera, the metal structure working condition in the pipeline is automatically identified. Based on the environmental determination result, the heading angle proportionality coefficient is dynamically adjusted. A higher first coefficient (such as 0.8) is taken in a strong electromagnetic interference environment to increase the weight of the north module, a medium second coefficient (such as 0.6) is used in a metal structure environment to balance the reliability of the data source, and the default coefficient (such as 0.3) is maintained in a normal working condition. Of course, the coefficient value can also be dynamically fine-tuned in a preset step (such as 0.05) according to the duration and intensity change rate of the interference, to realize smooth transition of the heading angle fusion.

[0063] S102: Use the complementary filtering algorithm to denoise the angular velocity data collected by the IMU, and obtain the compensated angular velocity.

[0064] After the pipeline robot enters the motion state, the inertial measurement unit IMU continuously collects angular velocity data at a frequency of 100 Hz. However, the original gyroscope angular velocity data has inherent defects: on the one hand, it contains high-frequency noise (caused by the sensor itself or environmental interference), and on the other hand, it has low-frequency zero drift (error that changes slowly over time). If these data are directly used for attitude integration, noise will cause attitude angle jitter, and drift will cause significant attitude error accumulated over time, which seriously affects the subsequent positioning accuracy.

[0065] To solve the above problems, the application adopts a complementary filtering algorithm to process the original angular velocity data collected by the IMU. Specifically, the gyroscope has good dynamic response and high accuracy in a short time (good high-frequency characteristics), but its output has integral drift (poor low-frequency characteristics); the accelerometer can provide a relatively accurate attitude reference in a stationary or slow motion state (good low-frequency characteristics), but its output contains non-gravitational acceleration interference when the robot is accelerating, and has poor dynamic response and high noise (poor high-frequency characteristics). The two types of data can be fused by the complementary filtering algorithm: the gyroscope data is high-pass filtered to retain its reliable high-frequency dynamic information and filter out the low-frequency drift component; the attitude information (mainly the pitch angle and roll angle) calculated by the accelerometer is low-pass filtered to extract its stable low-frequency attitude reference and filter out high-frequency noise and motion interference. Then, the high-pass filtered gyroscope angular velocity and the low-pass filtered accelerometer attitude information are superimposed and fused in the frequency domain.

[0066] In application, the original angular velocity data stream and the accelerometer data stream of the IMU are received in real time. First, the current pitch angle and roll angle of the robot are estimated in real time using the accelerometer data (the relationship of the gravity component in the navigation coordinate system). Then, the estimated angle is compared with the angle obtained by integrating the gyroscope to obtain an angle error. After the angle error passes through a proportional link (which can be regarded as a low-pass filter), a correction amount (compensation angular velocity) for the gyroscope measurement value is generated. Finally, the original angular velocity is added to the correction amount to obtain the compensation angular velocity. This compensation angular velocity not only retains the advantages of fast response and good dynamic performance of the gyroscope, but also effectively suppresses the zero drift of the gyroscope by using the low-frequency stability of the accelerometer. The coefficient of the proportional link determines the cutoff frequency of the filter, which can be optimized according to the sensor characteristics and application scenarios to balance the dynamic response speed and drift suppression effect.

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

[0068] In application, after obtaining the compensation angular velocity processed by the complementary filtering, the attitude updating process of the pipe robot is immediately launched. Based on the accurately compensated angular velocity information, the attitude angle of the robot in three-dimensional space can be calculated and updated in real time, and finally an attitude angle matrix representing the spatial orientation of the robot is formed.

[0069] Specifically, the attitude (i.e. pitch angle, roll angle and yaw angle) of the pipe robot changes continuously with time when it moves in the pipe. Using the discretized Euler angle differential equation, real-time iterative calculation can be performed. At each discrete time step (corresponding to the 100Hz sampling frequency of the IMU), the change in the attitude angle at the next time can be accurately calculated according to the attitude angle at the current time (initial attitude angle determined by step S101) and the measured compensation angular velocity at the time. By integrating and adding the change to the attitude angle at the previous time, the updated attitude angle at the current time, including the updated pitch angle, roll angle and yaw angle, can be obtained in real time. The attitude angle matrix, as the result of the attitude update, accurately describes the rotational transformation relationship between the carrier coordinate system (b system, fixed to the robot body) of the pipe robot and the navigation coordinate system (n system, referring to the northeast geographical coordinate system). Each element of the matrix is calculated by a specific trigonometric function combination of the updated real-time pitch angle, roll angle and yaw angle.

[0070] S104: Optimal estimation of the attitude angle matrix is performed using a nonlinear state estimation algorithm to obtain an optimized attitude angle matrix.

[0071] In applications, although the compensation angular velocity processed by the complementary filtering and the Euler angle method update can provide basic attitude information, in the actual motion process of the pipe robot, there are still many factors affecting the attitude accuracy. Sensor measurement noise, motion acceleration interference with gravity component perception, complex metal pipe wall environment disturbance to the magnetometer, and non-rigid vibration caused by the articulated structure of the robot itself, etc. will introduce errors in the attitude angle matrix. If these errors are not inhibited, they will accumulate over time, seriously affecting the accuracy of the final positioning result. In order to obtain higher precision and robustness of the attitude estimation, the nonlinear state estimation algorithm is introduced to further optimally estimate and correct the attitude angle matrix preliminarily updated by the Euler angle method.

[0072] Specifically, the nonlinear state estimation algorithm (which can be an extended Kalman filter algorithm or a particle filter algorithm) constructs a state vector containing attitude angle errors, sensor zero offset and other key parameters, and establishes a nonlinear state equation describing the evolution of these states over time. At the same time, the real-time accelerometer data and magnetometer data provided by the IMU are used as observation values to construct a nonlinear observation equation.

[0073] In the filtering iteration process, firstly, the optimal estimation state of the last moment is predicted based on the state equation to obtain the state prediction value and its covariance matrix at the current moment. Then, the actual observation value (accelerometer and magnetometer data) at the current moment is compared with the observation prediction value calculated according to the state prediction value to generate an observation residual. Then, the Kalman gain is calculated according to the covariance matrix (in the extended Kalman filter) or is updated by the particle weight (in the particle filter), and the observation residual is used to optimally correct the state prediction value to obtain the optimal estimation value of the state vector at the current moment. The optimal estimation value contains the optimized attitude angle error estimation.

[0074] Finally, the estimated attitude angle error is used to compensate or directly update the attitude angle matrix preliminarily updated by the Euler angle method, so as to obtain an optimized attitude angle matrix. The optimization process effectively integrates multi-source sensor information (gyroscope, accelerometer, magnetometer), dynamically estimates and corrects the attitude error introduced by sensor zero bias drift, motion acceleration interference, magnetometer disturbance and mechanical vibration, significantly suppresses the error accumulation effect, and greatly improves the long-term stability and accuracy of the attitude angle matrix, thereby laying a reliable foundation for subsequent accurate projection of the mileage increment into the navigation coordinate system for position calculation.

[0075] S105: consistency check is performed on the pulse data of the plurality of mileage wheels, and the corresponding effective pulse data is determined through weighted correction.

[0076] During the movement of the robot, the omnidirectional wheels located at different hinge positions will continuously generate pulse signals. These signals should theoretically be consistent and collectively reflect the actual displacement of the robot. However, under complex working conditions such as pipe water accumulation, sediment coverage, bends or variable diameters, some omnidirectional wheels may slip, resulting in abnormal pulse data and significant deviation from the data of other wheel groups. In order to identify and correct such abnormalities and ensure the accuracy of the mileage measurement, consistency check and fusion processing need to be performed on the pulse data from all omnidirectional wheels.

[0077] In one embodiment, the end of the head segment body unit and the end of the tail segment body unit are each provided with a passive wheel mechanism, and an elastic component is arranged between adjacent segment body units; the consistency check on the pulse data of the plurality of mileage wheels and the determination of the corresponding effective pulse data through weighted correction include:

[0078] determining the maximum difference value between each pair of pulse data;

[0079] if the maximum difference value is less than or equal to a preset threshold value, taking the average value of all pulse data as the effective pulse data;

[0080] if the maximum difference value is greater than the preset threshold value, acquiring the rotation signals of each passive wheel mechanism and the tension feedback data of each elastic component;

[0081] According to the rotation signal, the actual motion direction of the pipeline robot is determined, and the slipping odometer wheel is identified in combination with the tension feedback data;

[0082] Based on the tension feedback data, the pulse data corresponding to the slipping odometer wheel is weighted and corrected to generate effective pulse data.

[0083] In the application, first, the difference values between all pairs of omnidirectional wheel pulse data are calculated, and the maximum difference value is determined. The maximum difference value is compared with a preset threshold value. If the maximum difference value does not exceed the threshold value, it indicates that all omnidirectional wheels are in a normal working state and there is no obvious slipping, and at this time, the arithmetic mean value of all pulse data is taken as the final effective pulse data output. If the maximum difference value exceeds the preset threshold value, it indicates that one or more omnidirectional wheels are slipping or working abnormally. At this time, the rotation signal generated by the passive wheel mechanism and the real-time tension feedback data of each elastic component are obtained. The rotation signal of the passive wheel mechanism is used to assist in judging the actual overall motion direction of the pipeline robot. The tension feedback data of the elastic component directly reflects the normal pressure of the corresponding wheel group on the pipe wall; when the tension of the elastic component corresponding to a certain omnidirectional wheel is lower than a set safety threshold value, it can be determined that the wheel group is in a slipping state because it has lost sufficient friction force support.

[0084] After identifying the slipping omnidirectional wheel, the abnormal pulse data is weighted and corrected. Specifically, the pulse data corresponding to the slipping wheel group is given a lower weight (the weight value can be dynamically adjusted based on the degree to which the tension is lower than the threshold value or the deviation degree from other wheel group data), and the wheel group data in a normal state is given a higher weight. Then, all pulse data (including the corrected data of the slipping wheel) are weighted and averaged to generate fused effective pulse data. In this way, the negative impact of the abnormal data of the slipping wheel on the overall mileage calculation can be reduced.

[0085] S106: According to the effective pulse data, the mileage increment is determined.

[0086] After obtaining reliable effective pulse data through consistency checking and weighting correction, it needs to be converted into a physical quantity reflecting the actual displacement of the pipeline robot, i.e., the mileage increment. The mileage increment represents the distance moved by the pipeline robot along each axis in its own carrier coordinate system within a certain time period.

[0087] Specifically, each omni-wheel (odometer wheel) is equipped with a Hall magnetic switch. When the omni-wheel rolls on the inner wall of the pipeline, the magnetic steel embedded on the hub will periodically trigger the Hall magnetic switch, generating an electrical pulse signal. Each pulse corresponds to a fixed angle of rotation of the wheel, which is directly related to the number of magnetic steel arrangements on the hub. Therefore, by counting the number of valid pulses received within a fixed sampling time interval (for example, a 100 Hz sampling period synchronized with the IMU data), the actual arc length rolled by the omni-wheel in that time period can be accurately calculated.

[0088] Since the valid pulse data is the result of verification and correction, it integrates all the information of the normally working omni-wheels and suppresses the influence of the slipping wheels. When calculating the incremental distance, the arc lengths calculated by all the omni-wheels participating in the fusion (i.e., the wheel groups with non-zero weights) are weighted and averaged. The weights can be consistent with the weights assigned to the pulse data of each wheel group in step S105, i.e., the wheel groups with good status and sufficient tension have higher weights, and their calculation results have greater contribution to the final incremental distance. This weight-based fusion strategy can further reduce the influence of individual wheel manufacturing errors, slight slipping, or measurement noise on the overall displacement calculation.

[0089] Finally, the arc length calculated by this weighted average is the actual displacement of the pipeline robot in the forward direction in the carrier coordinate system within that time period, i.e., the incremental distance. This incremental distance reflects the true travel distance of the robot in the complex pipeline environment after overcoming the slipping interference of the wheel groups.

[0090] S107: Project the incremental distance 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.

[0091] After obtaining the accurate incremental distance and the attitude angle matrix optimized by the extended Kalman filter (EKF), the real-time position coordinates of the pipeline robot can be solved. In applications, the real-time position coordinates of the pipeline robot are determined in the navigation coordinate system by accumulating the displacement increments. The positioning process usually starts from a known initial position point; the coordinates of this initial position point, such as the starting point of the pipeline, can be measured by the global navigation satellite system (GNSS) at the entrance of the pipeline and recorded in the world coordinate system, and the coordinates in the navigation coordinate system can be obtained through the coordinate transformation relationship. Each time a new incremental distance is obtained and successfully projected into the navigation coordinate system, this displacement increment is added to the position coordinates at the previous time, thereby updating the real-time position coordinates of the pipeline robot in the navigation coordinate system at the current time. Through iterative calculation, the position coordinates of the robot are continuously updated as the robot travels, and finally the complete trajectory of the robot moving in the pipeline is formed.

[0092] In one embodiment, in the position updating stage, the pipe robot is rotated around the pipe axis by controlling the passive wheel mechanisms of the head and tail to rotate in the same direction; or the passive wheel mechanisms of the head and tail are controlled to rotate in opposite directions to adjust the included angle between the pipe robot and the pipe axis.

[0093] In the position updating stage of the robot performing the positioning task (i.e. the process of determining the real-time position coordinates by dead reckoning), the passive wheel mechanisms at the ends of the head and tail segments can be actively controlled to perform specific rotating actions to actively adjust the overall posture of the robot. Specifically, when the passive wheel mechanisms of the head and tail are controlled to rotate in the same direction (e.g. both clockwise or both counterclockwise), due to the frictional force between the passive wheels and the pipe wall, a torque is generated to rotate the robot as a whole around the pipe axis. This rotating motion does not change the axial position of the robot in the pipe, but can adjust the azimuth angle (i.e. the heading angle) around the central axis of the robot. This plays an important role when detection needs to be performed by changing the observation direction (such as adjusting the camera viewing angle), or when the heading deviation caused by cumulative errors needs to be corrected.

[0094] On the other hand, when the passive wheel mechanisms of the head and tail are controlled to rotate in opposite directions (e.g. the passive wheel of the head rotates clockwise and the passive wheel of the tail rotates counterclockwise, or vice versa), the frictional forces exerted by the passive wheels of the head and tail on the pipe wall are in opposite directions, thereby generating lateral forces in opposite directions at the two ends of the robot. The action of such forces causes the head segment and the tail segment of the robot to deflect relatively, thereby adjusting the included angle between the central axis of the robot and the pipe axis. This active adjustment of the included angle is crucial for the robot to smoothly pass through variable-diameter pipes, oblique joints or some complex non-standard connection parts. By adjusting this included angle in a timely manner, the robot can better adapt to the sudden changes in the geometry of the pipe, optimize the contact state between the odometer wheel and the pipe wall, reduce the risk of jamming, and ensure that the odometer wheel (omniwheel) maintains effective contact and rolling with the pipe wall as much as possible, thereby obtaining more reliable odometer data.

[0095] Referring to Figure 3 , Figure 3 The positioning method for the pipe robot is shown in the flowchart. In the application, the pipe robot is first placed in the pipe to be detected. The tension spring force of the elastic component (tension spring) causes the included angle between the first segment and the second segment, and the third segment and the fourth segment to have a tendency to elastically decrease, thereby driving the odometer wheel and the passive wheel mechanism to closely adhere to the inner wall of the pipe, and adjusting the segment included angle according to the pipe diameter. The walking process is shown in the schematic diagram Figure 4As shown. Secondly, the IMU (Inductively Coupled Memory) is mounted non-axially on the second section, sampling motion data at a frequency of 100Hz. The omnidirectional wheels generate pulse signals as the robot moves, with a sampling frequency of 10Hz. The two passive wheels at the front and rear contact and rotate with the inner wall of the pipe, and the active rotation direction controls the robot's rotation around the pipe axis. The data acquisition board receives and stores the data, and after the robot completes its movement, it uploads the data to the host computer via USB. Then, data processing is performed. Initial alignment is achieved using accelerometer and magnetometer data at rest, and the initial attitude angle is obtained after correcting the magnetic declination. During the attitude update phase, the real-time attitude angle is calculated using Euler angles based on gyroscope data. Complementary filtering is used to fuse accelerometer data to suppress drift, and further optimization is achieved using EKF (Extended Kinematics Function). In addition, odometer data processing is performed. First, the omnidirectional wheel pulse count is verified, and abnormal data is corrected by combining the passive wheel rotation signals and spring tension feedback. Then, position updates are performed. The odometer increment is projected onto the navigation coordinate system based on the updated attitude matrix, and the trajectory is calculated step-by-step starting from the starting point. Finally, the angle encoder, tension sensor, and IMU attitude information are monitored in real time throughout the process to compensate for mechanical structure errors. The sensor and calculation errors are optimized through algorithms to ultimately achieve high-precision positioning.

[0096] See Figure 5 , Figure 5 This is a schematic diagram of the data fusion positioning algorithm structure. First, the robot's attitude angles are updated, which is the foundation of trajectory estimation. Multi-step collaboration ensures the accuracy of the attitude angles. Specifically, the IMU is first used to complete initial alignment and obtain the initial attitude angles of the pipeline robot's body. This provides a starting reference for subsequent calculations. Next, the fuselage angular velocity is collected. After processing with a complementary filtering algorithm, this algorithm can effectively remove sensor noise, suppress inertial navigation integral drift, and output compensated angular velocities for attitude updates. Subsequently, the Euler angle method is used to update the initial attitude angles based on the compensated angular velocities. 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 make optimal estimates of the updated attitude angles, correct errors, ensure the reliability of attitude data, and provide an accurate attitude basis for position estimation.

[0097] In dead reckoning systems, odometers use incremental distance output, and the odometer coordinate system coincides with the vehicle coordinate system. It is assumed that the odometer is in... The measured distance increment within the time period is Its projection in the fuselage coordinate system {B} is After the attitude matrix is ​​updated, using... Attitude matrix of the carrier at time The odometer readings are projected from the vehicle coordinate system to the navigation coordinate system {N}. The position updating algorithm based on the distance increment is as follows: the starting point coordinate is The global position coordinate in the navigation coordinate system at time t is obtained through iterative calculation .

[0098] The positioning method flowchart of the present application is shown in Figure 6 ; first, the RTK real-time dynamic positioning technology and the GNSS global positioning system are used to obtain the three-dimensional coordinates of the pipeline starting point and endpoint in the world coordinate system, which anchors the global reference for subsequent positioning. Then, the wheel odometer pulse increment information is collected, and the IMU module is used to obtain the angular velocity, acceleration and attitude angle data in the robot motion in real time, and the positioning data basis is constructed. In the initial alignment stage, the robot first solves the initial pitch angle and roll angle, then combines the magnetometer measurement value to integrate the local magnetic declination information, and solves the initial heading angle to complete the initial attitude calibration. Subsequently, the attitude angle is solved by the Euler angle method, and then processed by the complementary filtering and extended Kalman filtering algorithm, to further optimize the body attitude angle and improve the attitude data precision. Finally, the optimized attitude angle matrix is combined to project the mileage increment into the navigation coordinate system to carry out position solving, generate the robot motion track, and then project the track into the world coordinate to complete the pipeline mapping and realize the construction of the global pipeline trajectory from the local motion data.

[0099] Reference is made to Figure 7 , Figure 7 for the schematic diagram of each coordinate system; Figure 7 The body coordinate system, the navigation coordinate system and the inertial navigation coordinate system are shown in the figure; among them, the body coordinate system (carrier coordinate system) takes the centroid or barycenter of the pipeline robot as the coordinate origin, and takes the forward direction of the pipeline robot as the positive direction of the X axis; the inertial navigation coordinate system takes the centroid or barycenter of the IMU as the coordinate origin, and takes the forward direction of the pipeline robot as the positive direction of the Y axis; in the application, the acceleration data and magnetic data are collected through the IMU, and these data can be projected from the inertial navigation coordinate system to the carrier coordinate system or the navigation coordinate system through coordinate transformation.

[0100] Corresponding to the foregoing application function implementation method embodiment, the present application also provides a pipeline robot positioning system based on the mileage wheel and the IMU and corresponding embodiments.

[0101] Reference is made to Figure 8 , Figure 8 for the module structure schematic diagram of the pipeline robot positioning system based on the mileage wheel and the IMU.

[0102] The pipeline robot comprises a first section body, a second section body, a third section body and a fourth section body connected in series, and mileage wheels are arranged on both sides of each hinge; the pipeline robot positioning system comprises:

[0103] The attitude angle determination unit 81 is configured to:

[0104] determine an initial attitude angle of the pipeline robot in a static state according to acceleration data and magnetic force data collected by the IMU;

[0105] de-noise the angular velocity data collected by the IMU by using a complementary filtering algorithm to obtain compensated angular velocity;

[0106] update the initial attitude angle by using an Euler angle method based on the compensated angular velocity to obtain an attitude angle matrix;

[0107] perform optimal estimation on the attitude angle matrix by using a nonlinear state estimation algorithm to obtain an optimized attitude angle matrix;

[0108] The distance determination unit 82 is configured to:

[0109] perform consistency check on the pulse data of the plurality of odometer wheels, and determine corresponding effective pulse data by weighted correction;

[0110] determine a distance increment according to the effective pulse data;

[0111] The position determination unit 83 is configured to project the distance 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.

[0112] In one embodiment, the ends of the first section body and the fourth section body are each provided with a passive wheel mechanism, and the first section body and the second section body and the third section body and the fourth section body are each provided with an elastic component; in terms of consistency check on the pulse data of the plurality of odometer wheels and determination of corresponding effective pulse data by weighted correction, the distance determination unit 82 is specifically configured to:

[0113] determine the maximum difference value between each pair of pulse data;

[0114] if the maximum difference value is less than or equal to a preset threshold value, taking the average value of all pulse data as the effective pulse data;

[0115] if the maximum difference value is greater than the preset threshold value, obtaining the rotation signals of each passive wheel mechanism and the tension feedback data of each elastic component;

[0116] determining the actual movement direction of the pipeline robot according to the rotation signals, and identifying the slipping odometer wheel in combination with the tension feedback data;

[0117] performing weighted correction on the pulse data corresponding to the slipping odometer wheel based on the tension feedback data to generate effective pulse data.

[0118] 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 the static state according to the acceleration data and the magnetic force data collected by the IMU, the attitude angle determination unit 81 is specifically configured to:

[0119] According to the gravity component in the inertial navigation coordinate system according to the acceleration data collected by the IMU, the initial pitch angle and the initial roll angle of the pipeline robot in the static state in the carrier coordinate system are calculated.

[0120] According to the local magnetic declination angle and the magnetic force data collected by the IMU, the initial heading angle is calculated.

[0121] As to the system in the above embodiment, the specific manner in which each module performs the operation has been described in detail in the embodiment related to the method, and will not be described in detail here.

[0122] The embodiment of the present application also provides a pipeline robot positioning device based on a mileage wheel and an IMU, including a processor and a memory, the memory stores a computer program, and the processor implements the positioning method described above when executing the computer program.

[0123] The positioning device (electronic device), please refer to Figure 9 The electronic device 9000 includes a memory 9010 and a processor 9020.

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

[0125] The memory 9010 can include various types of storage units, such as a system memory, a read-only memory (ROM), and a permanent storage device. Among them, the ROM can store static data or instructions required by the processor 9020 or other modules of the computer. The permanent storage device can be a readable and writable storage device. The permanent storage device can be a non-volatile storage device that does not lose stored instructions and data even after the computer is powered off. In some embodiments, the permanent storage device uses a mass storage device (such as a magnetic or optical disk, a flash memory) as a permanent storage device. In some other embodiments, the permanent storage device can be a removable storage device (such as a floppy disk, an optical drive). The system memory can be a readable and writable storage device or a volatile readable and writable storage device, such as a dynamic random access memory. The system memory can store some or all instructions and data required by the processor during runtime. In addition, the memory 9010 can include a combination of any computer readable storage media, including various types of semiconductor storage chips (such as DRAM, SRAM, SDRAM, flash memory, programmable read-only memory), magnetic disks and / or optical disks. In some embodiments, the memory 9010 can include a readable and / or writable removable storage device, such as a compact disc (CD), a read-only digital versatile disc (such as DVD-ROM, double-layer DVD-ROM), a read-only Blu-ray disc, an ultra-density optical disc, a flash memory card (such as an SD card, a min SD card, a Micro-SD card, etc.), a magnetic floppy disk, etc. The computer readable storage medium does not include a carrier wave and an instantaneous electronic signal transmitted by wireless or wired transmission.

[0126] The memory 9010 stores executable code, which, when processed by the processor 9020, can cause the processor 9020 to perform part or all of the above-mentioned methods.

[0127] In addition, the method according to the present application can also be implemented as a computer program or computer program product, which includes computer program code instructions for executing part or all of the above-mentioned steps of the method of the present application.

[0128] Alternatively, the present application can also be implemented as a computer readable storage medium (or non-transitory machine readable storage medium or machine readable storage medium) having executable code (or computer program or computer instruction code) stored thereon, which, when executed by a processor of an electronic device (or a server, etc.), causes the processor to execute part or all of the steps of the above-mentioned method according to the present application.

[0129] Having described various embodiments of the application, it is to be understood that the above description is meant to be illustrative only and not exhaustive, and is not limited to the disclosed embodiments. Many modifications and variations will be apparent to those of ordinary skill in the art, without departing from the scope and spirit of the described embodiments. The choice of words in this document is intended to best explain the principles of the embodiments, the practical application, or improvement over the technology in the art, or to enable others skilled in the art to understand the embodiments disclosed herein.

Claims

1. A method for positioning a pipe robot based on a wheeled odometer and an IMU, characterized by, The pipeline robot comprises a first section body, a second section body, a third section body and a fourth section body which are sequentially and serially connected in a hinged manner, and a plurality of odometers are arranged on both sides of each hinge; the method comprises the following steps: According to the acceleration data and the magnetic force data collected by the IMU, the initial attitude angle of the pipeline robot in the static state is determined; The complementary filtering algorithm is used to denoise the angular velocity data collected by the IMU to obtain compensated angular velocity; Based on the compensated angular velocity, the initial attitude angle is updated by using the Euler angle method to obtain an attitude angle matrix; The nonlinear state estimation algorithm is used to optimally estimate the attitude angle matrix to obtain an optimized attitude angle matrix; The pulse data of the plurality of odometers are subjected to consistency checking, and the corresponding effective pulse data are determined through weighted correction; According to the effective pulse data, the mileage increment is determined; 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; The end of each of the first section body and the fourth section body is provided with a passive wheel mechanism, and an elastic component is arranged between the first section body and the second section body and between the third section body and the fourth section body; the pulse data of the plurality of odometers are subjected to consistency checking, and the corresponding effective pulse data are determined through weighted correction, which comprises the following steps: The maximum difference value between each two of the pulse data is determined; If the maximum difference value is less than or equal to a preset threshold value, the average value of all the pulse data is taken as the effective pulse data; If the maximum difference value is greater than the preset threshold value, the rotation signals 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 according to the rotation signals, and the slipping odometer is identified in combination with the tension feedback data; The pulse data corresponding to the slipping odometer are subjected to weighted correction based on the tension feedback data to generate effective pulse data.

2. The odometer wheel and IMU based pipe robot positioning method according to claim 1, wherein, The initial attitude angle comprises an initial pitch angle, an initial roll angle and an initial heading angle; According to the acceleration data and the magnetic force data collected by the IMU, the initial attitude angle of the pipeline robot in the static state is determined, which comprises the following steps: According to the gravity component of the acceleration data collected by the IMU in the inertial navigation coordinate system, the initial pitch angle and the initial roll angle of the pipeline robot in the static state in the carrier coordinate system are solved; According to the local magnetic declination and the magnetic force data collected by the IMU, the initial heading angle is calculated.

3. The odometer wheel and IMU based pipe robot positioning method of claim 1, wherein, The IMU is fixedly installed on the third section body and is arranged offset from the axis of the pipeline robot.

4. The odometer wheel and IMU based pipe robot positioning method of claim 1, wherein, The nonlinear state estimation algorithm is specifically an extended Kalman filtering algorithm or a particle filtering algorithm.

5. A mile wheel and IMU based pipe robot positioning system, characterized in that, The pipeline robot comprises a first section body, a second section body, a third section body and a fourth section body which are sequentially and serially connected in a hinged manner, and a plurality of odometers are arranged on both sides of each hinge; the system comprises: An attitude angle determination unit is configured to: According to the acceleration data and the magnetic force data collected by the IMU, the initial attitude angle of the pipeline robot in the static state is determined; The IMU collected angular velocity data is denoised by using a complementary filtering algorithm to obtain compensated angular velocity; Based on the compensated angular velocity, the initial attitude angle is updated by using an Euler angle method to obtain an attitude angle matrix; The attitude angle matrix is optimally estimated by using a nonlinear state estimation algorithm to obtain an optimized attitude angle matrix; The mileage determination unit is configured to: Conduct consistency check on the pulse data of the plurality of mileage wheels, and determine corresponding effective pulse data through weighted correction; Determine the mileage increment according to the effective pulse data; The position determination unit is configured 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. The first section body and the fourth section body are each provided with a passive wheel mechanism, and the first section body and the second section body and the third section body and the fourth section body are each provided with an elastic component; in terms of consistency check on the pulse data of the plurality of mileage wheels and determination of corresponding effective pulse data through weighted correction, the mileage determination unit is specifically configured to: Determine the maximum difference value between each two pulse data; If the maximum difference value is less than or equal to a preset threshold value, the average value of all the pulse data is taken as the effective pulse data; If the maximum difference value is greater than the preset threshold value, the rotation signals of each passive wheel mechanism and the tension feedback data of each elastic component are obtained; Determine the actual movement direction of the pipeline robot according to the rotation signals, and identify the slipping mileage wheel in combination with the tension feedback data; Based on the tension feedback data, the pulse data corresponding to the slipping mileage wheel is weighted and corrected to generate effective pulse data.

6. A mile wheel and IMU based pipe robot positioning apparatus, characterized by, The positioning method comprises a processor and a memory, the memory stores a computer program, and the processor executes the computer program to realize the positioning method of claim 1.

7. A computer readable storage medium characterized in that, The computer program is stored on the memory and executed by the processor to realize the positioning method of claim 1.

Images