Pipeline detection robot and control method thereof

Abstract

The invention discloses a pipeline detection robot and a control method thereof, and belongs to the technical field of pipeline detection. A pipeline detection robot comprises a robot body, power devices installed at the two ends of a pipeline and a control unit located on the outer side of the pipeline, an inertial sensor is arranged in the robot body, each power device comprises a front air blower and a rear air blower, and the front air blower and the rear air blower are each provided with a control module. The control unit comprises a motion sensing unit and a motion control unit which is in electric signal connection with the motion sensing unit; according to the invention, the motion sensing ability and motion control of the inertial sensor arranged in the detection robot and an external power supply device are integrated, so that the detection robot does not need a built-in power system and an extra energy system, the size and weight of the robot are greatly reduced, and the robot can be easily applied to superfine and complex pipelines; and the applicability of the method in different pipeline scenes is remarkably improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of pipeline detection, and in particular relates to a pipeline detection robot and a control method thereof. Background Art

[0002] In modern industry and infrastructure maintenance, pipelines are key vehicles for the transmission of fluids, gases, and various media. Their safety and operational efficiency are crucial for ensuring the normal operation of production and daily life. With the increasing complexity and expansion of pipeline networks, especially the widespread use of small-diameter, long-distance, and multi-bend pipelines, regular pipeline inspection and maintenance has become an extremely challenging task. Traditional manual inspection methods are not only inefficient but also pose significant safety risks in complex, dangerous, or hard-to-reach environments.

[0003] In recent years, pipeline inspection robots, as an automated and intelligent inspection tool, have been widely used in the field of pipeline inspection. These robots can perform inspection tasks in complex and curved pipeline environments, effectively improving inspection efficiency and accuracy. However, in the actual application of pipeline inspection robots, how to achieve accurate perception and control of their motion state has become a technical problem that needs to be solved urgently.

[0004] Specifically, during operation, the pipeline inspection robot needs to accurately perceive its own motion state information such as position, speed and posture, so as to adjust the motion strategy according to the actual environment and ensure the smooth progress of the inspection task. At the same time, due to the complexity and uncertainty of the pipeline environment, the robot also needs to have strong motion control capabilities to cope with various emergencies and challenges.

[0005] Currently, existing pipeline inspection robots still have many deficiencies in perception and control. For example, some robots rely on post-processing to obtain inspection results and cannot provide real-time measurement results and positioning information, resulting in the inability to adjust motion strategies in a timely manner during operation. Furthermore, while some robots have real-time perception capabilities, their motion control is still limited by their own drive systems or external power supply methods, making it difficult to achieve accurate and efficient inspection tasks in complex, small-diameter pipelines. Summary of the Invention

[0006] The purpose of the present invention is to provide a pipeline inspection robot and a control method thereof in order to solve the problems existing in the prior art.

[0007] To achieve the above-mentioned objectives, the invention adopts the following technical solutions: a pipeline inspection robot, comprising a robot body, a power device installed at both ends of the pipeline, and a control unit located outside the pipeline, an inertial sensor is provided in the robot body, a first signal unit electrically connected to the inertial sensor is provided in the robot body, the power device comprises a front blower and a rear blower, the front blower and the rear blower are respectively provided with a control module, the front blower and the rear blower are respectively provided with a second signal unit electrically connected to the control module, the control unit comprises a motion sensing unit, and a motion control unit electrically connected to the motion sensing unit, the motion sensing unit is electrically connected to the second signal unit and the first signal unit.

[0008] By adopting the above-mentioned technical solution, the pipeline inspection robot of the present application constructs an efficient and flexible inspection system by integrating the robot body, the power devices at both ends of the pipeline and the outer control unit. The robot has built-in inertial sensors and signal units to sense and transmit the motion state in real time, providing data support for precise control. The power device adopts a dual-blower design to provide stable and adjustable propulsion force, and realizes real-time communication with the control unit through the control module and signal unit, which is convenient for dynamic adjustment. The motion perception and control unit in the control unit work together to fully sense and accurately control the robot movement, effectively solving the motion control and perception problems of traditional robots in complex small-diameter pipelines, and significantly improving the inspection efficiency and accuracy.

[0009] Optionally, a pipeline inspection robot, wherein the control method of the pipeline inspection robot comprises the following steps:

[0010] Placing the robot in the pipeline to be tested and installing the power device;

[0011] When the robot is running, the inertial sensor collects sensor data and position data and transmits them to the motion sensing unit, and the control module obtains wind data of the power device and transmits it to the motion sensing unit;

[0012] The motion sensing unit inputs the sensor data into a state space model for calculation, inputs the position data and the wind force data into an observation model for correction, estimates the actual motion state data and actual position data of the robot, and transmits the estimated data to the motion control unit;

[0013] The motion control unit uses a PID control algorithm to calculate the error between the real-time speed and the preset speed, and adjusts the total thrust of the power device in real time through a feedforward-feedback composite control method, and feeds the adjustment data back to the motion sensing unit;

[0014] The motion control unit generates a control instruction according to the adjustment data, and after receiving the control instruction, the control module adjusts the blowing or suction flow and wind direction to control the motion behavior of the robot;

[0015] During the movement of the robot, the motion sensing unit and the motion control unit continuously repeat the above steps to monitor the movement state of the robot in real time and dynamically adjust and optimize the control strategy according to the actual movement situation;

[0016] When the pipeline inspection task is completed, the power device stops working and the control unit performs a reset operation on the robot.

[0017] By adopting the above technical solution, the control method of the present application realizes real-time monitoring and dynamic adjustment of the robot's motion state through refined steps. After the robot is placed in the pipeline and the power unit is installed, the inertial sensor continuously collects and transmits data, and the power unit simultaneously provides wind data, providing rich information for motion state estimation. The motion perception unit uses the model to process data, accurately estimates the robot's state and position, and provides a decision basis for the control unit. The control unit adopts PID and feedforward-feedback composite control to adjust the thrust of the power unit in real time to ensure the stable operation of the robot. Through the cyclic perception and control steps, the continuous monitoring and optimization of the motion state is realized, effectively coping with the uncertainty of the pipeline environment. After the task is completed, the power unit stops, and the control unit resets the robot to prepare for the next inspection. This method significantly improves the automation and intelligence level of pipeline inspection, reduces manual intervention, and improves inspection efficiency and safety.

[0018] Optionally, the state space model includes a dynamic model, the acceleration of the robot is affected by airflow thrust and air resistance, and the calculation method of the dynamic model is:

[0019] ,

[0020] Where: is the true acceleration, m is the mass, is the estimated acceleration, c is the air resistance coefficient, is the acceleration error, is the speed error.

[0021] By adopting the above technical solution, the design of the dynamic model calculation method can more accurately characterize the robot's motion state in an airflow environment, incorporate the key factor of air resistance into the acceleration calculation, make the robot's motion model more in line with the actual situation, help improve the accuracy of the robot's motion perception, and thus provide a reliable basis for subsequent precise motion control.

[0022] Optionally, the state space model further includes an error transfer model, the error transfer model includes position error, velocity error, attitude error and sensor error, and the position error is calculated as follows:

[0023] ,

[0024] The speed error is calculated as follows: ,

[0025] The calculation method of the posture error is: ,

[0026] The calculation method of the sensor error is: ,

[0027] Where: is the position error at time k+1, k is the time, is the position error at time k, is the time interval, m is the mass, c is the air resistance coefficient, is the velocity error at time k, is the velocity error at time k+1, is the attitude error at time k+1, is the attitude error at time k, is the gyroscope bias error, is the angular velocity measurement noise, is the sensor error at time k+1, is the sensor error at time k, is the driving noise of the sensor deviation.

[0028] By adopting the above technical solutions, the design of calculation methods for position error, speed error, velocity error and sensor error helps to build a more accurate error transfer model, improve the accuracy and reliability of the robot's motion state perception, and provide strong support for the robot's precise control in complex environments.

[0029] Optionally, the state space model further includes a matrix model, wherein the matrix model includes a noise driving matrix and a Jacobian matrix, and the noise driving matrix is:

[0030] ,

[0031] The Jacobian matrix is: ,

[0032] Where: is the noise driving matrix, k is time, is the time interval, is the Jacobian matrix, m is the mass, c is the air resistance coefficient, is the cumulative value of the integral term.

[0033] By adopting the above technical solution, the noise driving matrix and the Jacobian matrix work together, enabling the matrix model to more comprehensively and accurately characterize the robot's motion state and error characteristics, effectively improving the robot's motion perception and control capabilities in complex environments.

[0034] Optionally, the observation model includes a position observation equation, and the calculation method of the position observation equation is:

[0035] ,

[0036] Where: k is time, is the position observation vector at time k, is the position observation matrix at time k, is one of the errors in the error transfer model, is the position observation noise at time k.

[0037] By adopting the above technical solution, the position observation equation helps to more accurately integrate observation data and system model information, improve the accuracy and reliability of the robot's position perception, and provide strong support for the robot's precise positioning and motion control.

[0038] Optionally, the observation model further includes a dynamic observation equation, and the calculation method of the dynamic observation equation is:

[0039] ,

[0040] Where: k is time, is the dynamic observation vector at time k, is the dynamic observation matrix at time k, is one of the errors in the error transfer model, is the dynamic observation noise at time k.

[0041] By adopting the above technical solution, the power observation equation can effectively integrate power observation data and system model information, improve the accuracy and reliability of the perception of the robot's power-related states, and provide a more accurate basis for the robot's power control and motion planning.

[0042] Optionally, the feedforward-feedback composite control method includes a calculation method for a feedforward term and a calculation method for a feedback adjustment term, and the calculation method for the feedforward term is:

[0043] ,

[0044] The calculation method of the feedback adjustment item is: ,

[0045] Where: is the feedforward thrust, c is the air resistance coefficient, is the preset speed, is the feedback thrust, is the proportional gain, is the speed error, is the integral gain, is the integral of the velocity error from 0 to time t, is the differential gain, is the rate of change of velocity error.

[0046] By adopting the above technical solution, the feedforward-feedback composite control method combines the rapid response of feedforward control and the precise adjustment advantages of feedback control, effectively improving the accuracy, stability and response speed of robot motion control.

[0047] Optionally, the total thrust of the power plant is calculated as follows:

[0048] ,

[0049] The calculation method of the discretized thrust is: ,

[0050] Where: is the airflow thrust, is the feedforward thrust, is the feedback thrust, c is the air resistance coefficient, is the preset speed, is the proportional gain, is the speed error, is the integral gain, is the integral of the velocity error from 0 to time t, is the differential gain, is the rate of change of velocity error, is the thrust command of the kth cycle, k is the time, is the speed error of the kth cycle, is the cumulative value of the integral term in the kth cycle, is the differential term of the kth period.

[0051] By adopting the above-mentioned technical solution, the setting of the total thrust calculation method of the power unit can adjust the thrust in real time to eliminate errors, so that the robot can quickly and accurately reach and maintain the preset speed, and improve the response speed and stability of motion control; at the same time, the discrete thrust calculation method can dynamically adjust the thrust according to the error situation of each cycle, making the control more precise and flexible, and adapting to the robot motion control needs in the actual digital control environment.

[0052] Compared with the prior art, the present invention has the following beneficial effects:

[0053] 1. This invention integrates the motion sensing capability of the inspection robot's built-in inertial sensor, motion control, and an external power supply device. This design eliminates the need for a built-in power system and an additional energy system for the inspection robot, significantly reducing the robot's size and weight. This allows the robot to be easily applied to ultra-thin and complex pipelines, significantly improving its applicability in different pipeline scenarios. At the same time, while reducing the size and weight, it still ensures precise control of the inspection robot, achieving a stable motion trajectory, and thereby improving the accuracy and reliability of pipeline measurement and inspection.

[0054] 2. Through bidirectional control of the external power supply device, the robot's motion state in the pipeline can be precisely adjusted to ensure its stability and flexibility. This feature effectively solves the problem of inaccurate measurement and detection caused by excessive speed or slow speed in traditional technologies, effectively ensuring the accuracy of the entire system, enabling the robot to perform inspection tasks in the pipeline more accurately.

[0055] 3. The state-space model includes a precise dynamic model that takes into account the effects of airflow thrust and air resistance on the robot's acceleration. This model can more accurately depict the robot's motion state in an airflow environment and improve the accuracy of motion perception. At the same time, the error transfer model includes calculation methods for position error, velocity error, attitude error, and sensor error, which helps to accurately estimate and correct various errors, further improving control accuracy.

[0056] 4. The position observation equation and dynamic observation equation in the observation model can effectively link the system state error with the observation data, use the observation information to estimate and correct the system state error, and by fusing the observation data with the system model information, improve the accuracy of the robot's position and dynamic state perception, providing strong support for precise control.

[0057] 5. The feedforward-feedback composite control method combines the rapid response of feedforward control and the precise adjustment of feedback control. The feedforward term can pre-compensate for external disturbances, and the feedback adjustment term can dynamically adjust the thrust according to the real-time error, effectively improving the accuracy, stability and response speed of the robot's motion control, ensuring the stable and efficient operation of the robot in the pipeline. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 This is a schematic diagram of the system operation of the present invention;

[0059] Figure 2 Schematic diagram of the feedback of the system of the present invention.

[0060] In the figure: 1. Pipeline; 2. Robot body; 3. Power unit; 31. Front blower; 32. Rear blower; 4. Control unit. DETAILED DESCRIPTION

[0061] The following will clearly and completely describe the technical solution of the present invention in conjunction with the accompanying drawings. Obviously, the embodiments described are only some embodiments of the present invention, not all 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.

[0062] In the description of the present invention, it should be noted that the terms "middle", "upper", "lower", "left", "right", "inside", "outside", etc., indicating directions or positional relationships, are based on the directions or positional relationships shown in the accompanying drawings. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operated in a specific direction. Therefore, they cannot be understood as limiting the present invention.

[0063] like Figure 1 As shown in FIG. 2 , a specific embodiment is as follows: a pipeline inspection robot comprises a robot body 2, a power device 3 installed at both ends of the pipeline 1, and a control unit 4 located outside the pipeline 1. The robot body 2 is provided with an inertial sensor, which includes an accelerometer, a gyroscope, etc., and is used to obtain and resolve the motion state information of the inspection robot in the pipeline 1 in real time, such as position, speed, acceleration, attitude angle, angular velocity, etc. The robot body 2 is provided with a first signal unit electrically connected to the inertial sensor. The first signal unit can ensure that the motion state information is transmitted to the control unit 4 in real time and quickly, so that the control unit 4 can monitor and control the movement of the robot based on the latest data.

[0064] The power device 3 includes a front blower 31 and a rear blower 32. The front blower 31 is used to detect the robot's forward movement, and the rear blower 32 is used to slow down or stop the operation by providing reverse airflow. The front blower 31 and the rear blower 32 both have bidirectional functions of blowing and sucking to achieve bidirectional movement of the detection robot and cooperate to obtain stronger power and more precise control effects. The front blower 31 and the rear blower 32 are respectively provided with a control module. The control module can adjust the intensity and direction of the airflow according to the instructions issued by the control unit 4, thereby accurately adjusting the movement speed and direction of the detection robot. The front blower 31 and the rear blower 32 are respectively provided with a second signal unit electrically connected to the control module. The second signal unit can provide real-time feedback to the control unit 4 on relevant information of the power device 3, such as working status parameters such as airflow intensity and direction, which enables the control unit 4 to timely understand the actual operation of the power device 3 and provide a basis for subsequent precise control.

[0065] The control unit 4 includes a motion sensing unit and a motion control unit 4 electrically connected to the motion sensing unit. The motion sensing unit accurately estimates the motion state of the detection robot through the established state space model and observation model. The motion control unit 4 can accurately calculate the current position information and motion state of the device, and transmit these data to the motion control algorithm, generate control instructions to the external power supply device, so as to adjust the motion behavior of the detection robot in real time. The motion sensing unit is electrically connected to the second signal unit and the first signal unit.

[0066] In some implementations, the pipeline 1 inspection robot is equipped with sensors such as a magnetometer, a camera, a radar, and an odometer to provide additional information to assist in obtaining the motion status information of the inspection robot in the pipeline 1 .

[0067] A pipeline inspection robot, wherein the control method of the pipeline inspection robot comprises the following steps:

[0068] The detection robot is placed in the pipeline 1 to be tested, and a front blower 31 and a rear blower 32 are installed at both ends of the pipeline 1 to provide power;

[0069] During the inspection of the pipeline 1 by the inspection robot, the inertial sensor collects sensor data and position data of the inspection robot in real time and transmits the data to the motion sensing unit through the first signal unit. The control module obtains wind speed data of the front blower 31 and the rear blower 32 in real time and transmits the data to the motion sensing unit through the second signal unit.

[0070] The motion sensing unit inputs the sensor data into a state space model for calculation, and the motion sensing unit inputs the position data and the wind force data into an observation model for correction, to estimate the actual motion state data and actual position data of the robot;

[0071] The motion sensing unit transmits the actual motion state data and the actual position data to the motion control unit 4;

[0072] The motion control unit 4 uses a PID control algorithm to compare the real-time speed of the detection robot given by the motion sensing unit with the preset speed (or the real-time position with the preset position) and calculate the speed error;

[0073] According to the speed error, the total thrust of the power unit 3 is adjusted in real time through a feedforward-feedback composite control method, and the adjusted thrust is fed back to the motion sensing unit;

[0074] The motion control unit 4 generates corresponding control instructions according to the above control strategy. After receiving the control instructions, the control modules of the front blower 31 and the rear blower 32 adjust their own blowing or suction flow and wind direction to achieve real-time control of the motion behavior of the detection robot.

[0075] During the process of detecting the robot's motion, the motion sensing unit and the motion control unit 4 continuously repeat the above-mentioned cycle of signal acquisition, state estimation, control instruction generation and execution, monitor the robot's motion state in real time, and dynamically adjust and optimize the control strategy according to the actual motion situation;

[0076] If the detection robot moves forward too fast, the front blower 31 can slow down by reducing the blowing flow or even inhaling air; if the speed is too slow, the front blower 31 can accelerate by increasing the blowing flow. At the same time, the rear blower 32 provides reverse airflow as needed to slow down or stop the operation, or assists the front blower 31 in providing forward thrust when necessary, so as to ensure that the detection robot always maintains the predetermined movement speed range and flexibly adapts to different pipeline 1 environments and complex movement requirements.

[0077] When the pipeline 1 detection task is completed, the motion control unit 4 will determine whether the task is completed based on the preset end conditions (such as the detection distance reaches the set value, the end of the pipeline 1 is detected, etc.);

[0078] Once the motion control unit 4 determines that the task is completed according to the preset end conditions, it will send a shutdown command to the front blower 31 and the rear blower 32. The power unit 3 stops working after receiving the shutdown command, and the control unit 4 resets the detection robot.

[0079] Before using the inspection robot, a state space model and an observation model must be established in advance according to the inspection robot's usage scenario. The establishment of the state space model and the observation model includes the following steps:

[0080] 1. Error state vector definition

[0081] Before establishing the state space model and observation model, it is necessary to clearly define the actual state and estimated state of the system.

[0082] Assume the true state is , the current estimated state is , where: is the true state vector at time k, is the real three-dimensional position vector, is the real three-dimensional velocity vector, is the true 3D pose vector, is the estimated value of the state vector at time k, is the estimated position, is the estimated value of velocity, is the estimated value of the posture;

[0083] The system error state is defined as a small deviation to measure the difference between the estimated state and the true state. The error state vector δxk is defined as:

[0084] ,

[0085] Where: is the error state vector at time k;

[0086] The meaning of each part is as follows:

[0087] Position error: (3D),

[0088] Speed ​​error: (3D),

[0089] Attitude error: (3D),

[0090] Sensor error: (IMU zero bias, six dimensions: gyroscope + accelerometer).

[0091] The prediction of the true state is achieved through the original dynamic model, and the error state transfer is obtained by Taylor expansion at the estimated state and retaining the first-order terms:

[0092] ,

[0093] Where: is the error state vector at time k+1, is the error state vector at time k, is the state transition matrix, is the noise driving matrix, is the process noise vector.

[0094] 2. Construct a state-space model, which includes a dynamic model, position error transfer, velocity error transfer, attitude error transfer, sensor error transfer, noise drive matrix and Jacobian matrix.

[0095] (1) Construction of dynamic model

[0096] The state space model includes a dynamic model. The acceleration of the robot is affected by airflow thrust and air resistance. It is assumed that air resistance is proportional to speed and can be expressed as , according to Newton's second law, we can get , the calculation formula of true acceleration is: ,

[0097] The formula for calculating the estimated acceleration is: ,

[0098] Therefore, the relationship between acceleration measurement error and velocity measurement error is: ,

[0099] Where: is the airflow thrust, c is the air resistance coefficient, v is the velocity, a is the acceleration, is the true acceleration, m is the mass, is the estimated acceleration, c is the air resistance coefficient, is the true speed, To estimate the speed, is the acceleration error, is the speed error.

[0100] (2) Position error transfer

[0101] The position error update is driven by both the velocity error and the acceleration error (implied in the velocity):

[0102] ,

[0103] Right now, ,

[0104] Where: is the position error at time k+1, is the position error at time k, is the velocity error at time k, is the time interval, is the acceleration error at time k.

[0105] (3) Speed ​​error transfer

[0106] The velocity error is updated by the acceleration error (i.e. the velocity error itself):

[0107] ,

[0108] Right now, ,

[0109] In the formula is the velocity error at time k+1.

[0110] (4) Attitude error transfer

[0111] Assuming the attitude error is driven by gyroscope bias and angular rate noise:

[0112] ,

[0113] Where: is the attitude error at time k+1, is the gyroscope bias error (included in middle), is the angular velocity measurement noise.

[0114] (5) Sensor error transfer

[0115] Sensor bias is usually assumed to be a random walk process, i.e., ,

[0116] Where: is the sensor error at time k+1, is the driving noise of the sensor deviation.

[0117] (6) Noise driving matrix

[0118] Process noise Including velocity noise (from thrust uncertainty or drag coefficient error), angular velocity noise , sensor bias driven noise ,

[0119] Noise Driven Matrix for: ,

[0120] Noise covariance matrix Need to be calibrated according to the actual system: ,

[0121] Where: is the cumulative value of the integral term.

[0122] (7) Jacobian matrix

[0123] State transition matrix The dimension is , the block format is: ,

[0124] Specific block description:

[0125] Partial derivative of position error with respect to velocity error: ,

[0126] Partial derivative of velocity error with respect to itself: ,

[0127] Partial derivative of attitude error to gyro bias: ,

[0128] The other blocks are identity matrices or zero matrices.

[0129] 3. Construct an observation model, which includes position observation equations and dynamic observation equations.

[0130] (1) Position observation equation

[0131] When the robot is at the entrance or exit of pipe 1, the actual position is known , the observed value is the deviation between the estimated position and the true position, that is, the position error: ,

[0132] The observation equation can be expressed as a linear relationship , where the observation matrix only extracts the position error part:

[0133] ,

[0134] Where: is the position observation vector at time k, is the estimated position at time k, is the position error in the state error vector at time k (i.e. ), the first 3 dimensions of is the position observation noise at time k, the covariance matrix (diagonal matrix, elements are the noise variance of each axis position measurement), the first block Corresponding position error , the rest of the blocks are zero matrices (independent of velocity error, attitude error, and sensor bias), is the position observation matrix at time k, is one of the errors in the error transfer model, is the position observation noise at time k.

[0135] (2) Dynamic observation equation (known wind force)

[0136] Known wind force (airflow thrust) provided to the robot , according to Newton's second law, the deviation between the true acceleration and the estimated acceleration is: ,

[0137] The core of dynamic observation is to calculate the theoretical acceleration by knowing the wind force and compare it with the acceleration predicted by the model. The observed value is the acceleration error (which indirectly reflects the speed error): ,

[0138] The observation equation can be expressed as a linear relationship , where the observation matrix only extracts the velocity error part and multiplies it by the coefficient: , among which, the second block Corresponding speed error , the rest of the blocks are zero matrices (independent of position error, attitude error, and sensor bias),

[0139] Where, is the dynamic observation vector at time k, is the velocity error in the state error vector at time k (i.e. 4-6 dimensions), is the dynamic observation noise at time k, the covariance matrix (diagonal array, reflecting wind measurement error or uncertainty of drag coefficient c),

[0140] 4. Here we take the forward speed of the pipeline robot as an example to design the control algorithm (the position control algorithm is the same). The control goal is to ensure that the actual speed of the robot Tracking preset speed , the control input is the airflow thrust , PID algorithm is used to generate thrust command to compensate for speed error.

[0141] (1) Definition of speed error

[0142] The real-time speed error based on state estimation is: , where: is the speed error.

[0143] (2) Feedforward-feedback composite control

[0144] Feedforward term: offsets the steady-state drag and directly calculates the theoretical thrust required to maintain the target speed: ,

[0145] Feedback adjustment item: PID compensation dynamic error: ,

[0146] Where: is the feedforward thrust, is the feedback thrust, is the proportional gain, is the integral gain, is the differential gain.

[0147] (3) Total thrust command

[0148] ,

[0149] Discrete time implementation, assuming the control period is , the control law after discretization is:

[0150] Error calculation: ,

[0151] Integral term accumulation: ,

[0152] Differential term difference: ,

[0153] Thrust command generation: ,

[0154] Where: is a discrete time step, is the speed error of the kth cycle, is the cumulative value of the integral term in the kth cycle, is the differential term of the kth period, is the thrust instruction of the kth cycle.

[0155] (4) Anti-saturation and filtering

[0156] Integral Limiter: Limit the integral term amplitude to prevent Windup: ,

[0157] Differential filtering: Apply a first-order low-pass filter to the differential term (time constant ): ,

[0158] Where: is the integral limiting function, is the minimum / maximum allowed value of the integral term, is the differential term after filtering, is the filter coefficient, is the time constant of the low-pass filter.

[0159] (5) Parameter tuning suggestions

[0160] Proportional gain : Determines the response speed. Too large will cause overshoot, too small will cause slow response.

[0161] Integral gain : Eliminate steady-state error, but avoid integral saturation.

[0162] Differential gain : Suppress oscillation, but be aware of noise sensitivity.

[0163] Feedforward gain :Derived directly from the physical model ( ).

[0164] (6) Coupling with state estimation

[0165] Input interface: The controller receives the output of the state estimator .

[0166] Output interface: thrust command As the input of the dynamic model, the closed-loop control process is as follows:

[0167] The difference between the preset speed and the speed estimate generates an error, which is synthesized with the feedforward thrust after PID adjustment to generate the total thrust, and the drive system generates the real speed. The error is then fed back to the error calculation through state estimation, forming a closed loop of "error feedback → control correction → state perception → re-feedback", integrating feedforward prediction and feedback correction to achieve precise control.

[0168] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A pipeline inspection robot, characterized in that: The invention comprises a robot body, a power device installed at both ends of the pipeline, and a control unit located outside the pipeline, wherein an inertial sensor is provided in the robot body, and a first signal unit electrically connected to the inertial sensor is provided in the robot body, the power device comprises a front blower and a rear blower, the front blower and the rear blower are respectively provided with a control module, the front blower and the rear blower are respectively provided with a second signal unit electrically connected to the control module, the control unit comprises a motion sensing unit, and a motion control unit electrically connected to the motion sensing unit, and the motion sensing unit is electrically connected to the second signal unit and the first signal unit.

2. A pipeline inspection robot according to claim 1, characterized in that: The control method of the pipeline detection robot includes the following steps: Placing the robot in the pipeline to be tested and installing the power device; When the robot is running, the inertial sensor collects sensor data and position data and transmits them to the motion sensing unit, and the control module obtains wind data of the power device and transmits it to the motion sensing unit; The motion sensing unit inputs the sensor data into a state space model for calculation, inputs the position data and the wind force data into an observation model for correction, estimates the actual motion state data and actual position data of the robot, and transmits the estimated data to the motion control unit; The motion control unit uses a PID control algorithm to calculate the error between the real-time speed and the preset speed, and adjusts the total thrust of the power device in real time through a feedforward-feedback composite control method, and feeds the adjustment data back to the motion sensing unit; The motion control unit generates a control instruction according to the adjustment data, and after receiving the control instruction, the control module adjusts the blowing or suction flow and wind direction to control the motion behavior of the robot; During the movement of the robot, the motion sensing unit and the motion control unit continuously repeat the above steps to monitor the movement state of the robot in real time and dynamically adjust and optimize the control strategy according to the actual movement situation; When the pipeline inspection task is completed, the power device stops working and the control unit performs a reset operation on the robot.

3. The control method of a pipeline inspection robot according to claim 2, characterized in that: The state space model includes a dynamic model. The acceleration of the robot is affected by airflow thrust and air resistance. The calculation method of the dynamic model is: ， Where: is the true acceleration, m is the mass, is the estimated acceleration, c is the air resistance coefficient, is the acceleration error, is the speed error.

4. The control method of a pipeline inspection robot according to claim 2, characterized in that: The state space model also includes an error transfer model, which includes position error, velocity error, attitude error and sensor error. The position error is calculated as follows: ， The speed error is calculated as follows: , The calculation method of the posture error is: , The calculation method of the sensor error is: , Where: is the position error at time k+1, k is the time, is the position error at time k, is the time interval, m is the mass, c is the air resistance coefficient, is the velocity error at time k, is the velocity error at time k+1, is the attitude error at time k+1, is the attitude error at time k, is the gyroscope bias error, is the angular velocity measurement noise, is the sensor error at time k+1, is the sensor error at time k, is the driving noise of the sensor deviation.

5. The control method of a pipeline inspection robot according to claim 2, characterized in that: The state space model further includes a matrix model, which includes a noise driving matrix and a Jacobian matrix. The noise driving matrix is: ， The Jacobian matrix is: , Where: is the noise driving matrix, k is time, is the time interval, is the Jacobian matrix, m is the mass, c is the air resistance coefficient, is the cumulative value of the integral term.

6. The control method of a pipeline inspection robot according to claim 2, characterized in that: The observation model includes a position observation equation, and the calculation method of the position observation equation is: ， Where: k is time, is the position observation vector at time k, is the position observation matrix at time k, is one of the errors in the error transfer model, is the position observation noise at time k.

7. The control method of a pipeline inspection robot according to claim 2, characterized in that: The observation model also includes a dynamic observation equation, and the calculation method of the dynamic observation equation is: ， Where: k is time, is the dynamic observation vector at time k, is the dynamic observation matrix at time k, is one of the errors in the error transfer model, is the dynamic observation noise at time k.

8. The control method of a pipeline inspection robot according to claim 2, characterized in that: The feedforward-feedback composite control method includes a calculation method for a feedforward term and a calculation method for a feedback adjustment term. The calculation method for the feedforward term is: ， The calculation method of the feedback adjustment item is: , Where: is the feedforward thrust, c is the air resistance coefficient, is the preset speed, is the feedback thrust, is the proportional gain, is the speed error, is the integral gain, is the integral of the velocity error from 0 to time t, is the differential gain, is the rate of change of velocity error.

9. The control method of a pipeline inspection robot according to claim 2, characterized in that: The total thrust of the power plant is calculated as follows: ， The calculation method of the discretized thrust is: , Where: is the airflow thrust, is the feedforward thrust, is the feedback thrust, c is the air resistance coefficient, is the preset speed, is the proportional gain, is the speed error, is the integral gain, is the integral of the velocity error from 0 to time t, is the differential gain, is the rate of change of velocity error, is the thrust command of the kth cycle, k is the time, is the speed error of the kth cycle, is the cumulative value of the integral term in the kth cycle, is the differential term of the kth period.

Images