Snakelike pipeline detector, control method and device and storage medium
By designing a snake-shaped pipeline detector equipped with sonar, camera and thruster, the problem of traditional robots being difficult to pass complex pipelines is solved, and stable detection is achieved under high flow velocity and high pressure environments is improved, and detection efficiency and practicality are improved.
Patent Information
- Application Number
- CN202510260829.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-03-06
- Publication Date
- 2025-06-27
AI Technical Summary
Due to its rigid structure, traditional underwater frame robots are difficult to pass through narrow or complex pipelines, limiting the detection efficiency inside the pipeline.
A snake-shaped pipeline detector is designed, including 5 cabins, equipped with sonar, camera, thruster and nine-axis sensor. Through a specific propulsion structure and sensing structure, the detector can hover, advance, yaw, roll, adhere to the wall, and roll with the wall in a high flow rate and high pressure environment.
It realizes stable and efficient inspection in complex pipeline environments, improves the working posture stability and detection practicality of the detector, and meets the actual pressure detection needs that do not affect residents' water use.
Smart Images
Figure CN120212359A_ABST
Abstract
Description
Technical Field
[0001] This application relates to the technical field of pipeline detection, and particularly to a snake-shaped pipeline detector, a control method, a device and a storage medium. Background Art
[0002] With the rapid development of urban economy, the increasing demand for water supply has brought increasingly serious pressure to urban water supply pipe networks. Long-distance and large-diameter water supply main pipelines need to meet the water supply requirements of high pressure and high flow rate. Inevitably, they will pass through areas with uneven terrain during deployment. The water hammer and airbag effects in the pipeline will cause intense, continuous and frequent impacts on the inner wall of the pipe. Therefore, pipeline accidents such as pipeline leakage, settlement and even pipe explosion are more likely to occur.
[0003] In order to avoid property losses to urban construction and people's livelihood and health caused by pipeline accidents, it is very necessary to regularly detect the inside of the pipe network. Traditional underwater frame robots are often limited by their rigid structures when operating inside narrow or complex pipelines, and thus cannot pass through various curved or irregular pipelines, seriously restricting the detection efficiency. Summary of the Invention
[0004] This application provides a snake-shaped pipeline detector, a control method, a device and a storage medium, which are used to adapt to complex pipeline environments and complete the internal detection of pipe network pipelines.
[0005] To achieve the above object, this application adopts the following technical solutions:
[0006] In the first aspect, a snake-shaped pipeline detector is provided, which includes 5 compartments; the first compartment is equipped with a sonar and a camera, the second compartment is equipped with a first thruster and a second thruster, the third compartment is equipped with a third thruster and a fourth thruster, the fourth compartment is equipped with a nine-axis sensor, a fifth thruster and a sixth thruster, the fifth compartment is equipped with a seventh thruster, the nine-axis sensor is located at the centroid of the snake-shaped pipeline detector, and the nine-axis sensor includes a three-axis gyroscope, a three-axis accelerometer and a three-axis digital compass. The sonar is used to collect the distance between the pipe wall at each scanning angle, the camera is used to collect the image data inside the pipeline, and the nine-axis sensor is used to collect the attitude of the snake-shaped pipeline detector. Taking the centroid as the origin and the forward direction of the snake-shaped pipeline detector as the positive x-axis direction, a Cartesian coordinate system is defined based on the right-hand rule; the first thruster and the second thruster are located in the x-y plane, and the propulsion direction is parallel to the x-axis, the third thruster and the fourth thruster form a 45° angle with the x-y plane, and the propulsion direction is parallel to the y-z plane, the fifth thruster and the sixth thruster are located in the x-z plane, and the propulsion direction is parallel to the x-axis, and the seventh thruster is located in the x-y plane, and the propulsion direction is parallel to the z-axis.
[0007] In a second aspect, a control method is provided. This control method can be applied to the serpentine pipeline detector proposed in the first aspect. The method includes: obtaining an input control instruction, where the control instruction includes a pitch angle offset value, a roll angle offset value, a yaw angle offset value, and a target force. Determining a target attitude based on the pitch angle offset value, the roll angle offset value, the yaw angle offset value, and the historical attitude of the serpentine pipeline detector. Obtaining the measured attitude of the serpentine pipeline detector, where the measured attitude is the attitude collected by a nine-axis sensor at the current moment. Determining an expected torque based on the target attitude and the measured attitude. Obtaining a plurality of distances collected by a sonar, where the plurality of distances are the distances between the sonar and the pipe wall in each scanning angle direction. Correcting the target force based on the plurality of distances to obtain an expected force. Determining the propeller speed corresponding to each thruster based on the expected torque and the expected force, and controlling the corresponding thruster to operate based on each propeller speed.
[0008] In a possible design, the historical attitude includes a historical pitch angle, a historical roll angle, and a historical yaw angle, and the target attitude includes a target pitch angle, a target roll angle, and a target yaw angle. The expression for determining the target attitude based on the pitch angle offset value, the roll angle offset value, the yaw angle offset value, and the historical attitude of the serpentine pipeline detector is:
[0009] Θ = Θ′ + [Δφ Δθ Δψ] T ;
[0010] [φ θ ψ] T = [φ′ θ′ ψ′] T +[Δφ Δθ Δψ] T ;
[0011] In the formula: Θ is the target attitude, Θ′ is the historical attitude, Δφ is the pitch angle offset value, Δθ is the roll angle offset value, Δψ is the yaw angle offset value, φ is the target pitch angle, θ is the target roll angle, ψ is the target yaw angle, φ′ is the historical pitch angle, θ′ is the historical roll angle, and ψ′ is the historical yaw angle.
[0012] In a possible design, determining the expected torque based on the target attitude and the measured attitude includes: determining an expected angular velocity based on the target attitude, the measured attitude, and a proportional-integral-derivative (PID) control law; determining an expected output angular velocity based on the expected angular velocity and the measured angular velocity, where the measured angular velocity is the angular velocity collected by a nine-axis sensor at the current moment; and determining the expected torque based on the expected output angular velocity and the moment of inertia of the serpentine pipeline detector.
[0013] In a possible design, the expression for determining the expected angular velocity based on the target attitude, the measured attitude, and the proportional-integral-derivative (PID) control law is:
[0014]
[0015] e = Θ - Θ m ;
[0016] where: Θ is the target attitude, Θ m is the measured attitude, ω is the desired angular velocity, K P is the proportionality coefficient, K i is the integral coefficient, K D is the differential coefficient, the variable k is used to represent the current moment, and e is the difference between the target attitude and the measured attitude.
[0017] In a possible design, the target force is corrected according to multiple distances to obtain the desired force, including: dividing the multiple distances into distance arrays corresponding to four directions to obtain a first distance array, a second distance array, a third distance array, and a fourth distance array; the distances included in the first distance array are the distances collected by the sonar when the scanning angles are 1°, 2°... 44°, and 315°, 316°... 360°, the distances included in the second distance array are the distances collected by the sonar when the scanning angles are 45°, 46°... 134°, the distances included in the third distance array are the distances collected by the sonar when the scanning angles are 135°, 136°... 224°, and the distances included in the fourth distance array are the distances collected by the sonar when the scanning angles are 225°, 226°... 314°; according to the direction of the target force, the target distance array is determined, and the direction corresponding to the target distance array is the direction of the target force; the target force is corrected according to the minimum distance in the target distance array to obtain the desired force.
[0018] In a possible design, the expression for correcting the target force according to the minimum distance in the target distance array to obtain the desired force is:
[0019]
[0020] where: F′ is the desired force, F is the target force, is the first distance threshold, is the second distance threshold, the first distance threshold is greater than the second distance threshold, D is the minimum distance in the target distance array, F d is a preset value.
[0021] In a possible design, the expression for determining the propeller speed corresponding to each thruster according to the desired torque and the desired force is:
[0022]
[0023] where: M is the control efficiency matrix, τ is the desired torque, f′ is the desired force, The rotational speeds of the propellers of the first thruster, the second thruster, the third thruster, the fourth thruster, the fifth thruster, the sixth thruster, and the seventh thruster, respectively, are τ x , τ y , τ z The torques corresponding to the serpentine pipeline detector on the x, y, and z axes are f x is the force of the desired force in the x-axis direction, f′ y is the force of the desired force in the y-axis direction, f′ z is the force of the desired force in the z-axis direction.
[0024] In a third aspect, a control device is provided, which is deployed in the serpentine pipeline detector proposed in the first aspect. The control device includes an acquisition unit, a determination unit, and a processing unit. The acquisition unit is used to acquire the input control instruction, and the control instruction includes the pitch angle offset value, the roll angle offset value, the yaw angle offset value, and the target force. The determination unit is used to determine the target attitude according to the pitch angle offset value, the roll angle offset value, the yaw angle offset value, and the historical attitude of the serpentine pipeline detector. The acquisition unit is further used to acquire the measured attitude of the serpentine pipeline detector, and the measured attitude is the attitude collected by the nine-axis sensor at the current moment. The determination unit is further used to determine the desired torque according to the target attitude and the measured attitude. The acquisition unit is further used to acquire a plurality of distances collected by the sonar, and the plurality of distances are the distances between the sonar and the pipe wall at each scanning angle. The processing unit is used to correct the target force according to the plurality of distances to obtain the desired force. The determination unit is further used to determine the rotational speed of the propeller corresponding to each thruster according to the desired torque and the desired force. The processing unit is further used to control the corresponding thruster to operate based on each rotational speed of the propeller.
[0025] In a fourth aspect, the present application provides a computer-readable storage medium, which includes a stored program. When the program runs, it controls the device where the computer-readable storage medium is located to execute the control method provided in the second aspect above.
[0026] In the serpentine pipeline detector and the control method proposed in the present application, based on a specific propulsion structure and sensing structure, the serpentine pipeline detector can realize the motion functions of stable hovering, forward movement, yaw, roll, wall-following forward movement, and wall-following roll in the working environment of a large-diameter raw water pipeline with high flow rate and high pressure, improving the stability of the working attitude of the serpentine pipeline detector and the practicality of detection, and meeting the actual pressure detection requirements without affecting the daily water use of residents. BRIEF DESCRIPTION OF THE DRAWINGS
[0027] Figure 1 FIG. is a schematic structural diagram of a serpentine pipeline detector provided by an embodiment of the present application;
[0028] Figure 2Schematic diagram of the control flow of a snake-shaped pipeline detector provided by an embodiment of the present application;
[0029] Figure 3 Schematic diagram of the flow of a control method provided by an embodiment of the present application;
[0030] Figure 4 Schematic diagram of the flow of another control method provided by an embodiment of the present application;
[0031] Figure 5 Schematic diagram of the flow of another control method provided by an embodiment of the present application;
[0032] Figure 6 Schematic diagram of the structure of a control device provided by an embodiment of the present application. Detailed implementation manners
[0033] Next, the technical solutions in the embodiments of the present application will be described with reference to the accompanying drawings in the embodiments of the present application.
[0034] In the embodiments of the present application, words such as "exemplary" or "for example" are used to represent examples, illustrations or explanations. Any embodiment or design solution described as "exemplary" or "for example" in the embodiments of the present application should not be construed as being more preferred or having more advantages than other embodiments or design solutions. Exactly speaking, using words such as "exemplary" or "for example" is intended to present relevant concepts in a specific manner.
[0035] In the description of the present application, unless otherwise specified, " / " means "or". For example, A / B may represent A or B. The "and / or" herein is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B may represent: A exists alone, A and B exist simultaneously, and B exists alone. In addition, "at least one" and "multiple" refer to two or more. The words such as "first" and "second" do not limit the quantity and execution order, and the words such as "first" and "second" do not necessarily limit being different.
[0036] Embodiment 1
[0037] The embodiments of the present application provide a snake-shaped pipeline detector, the structure of which is as Figure 1 shown, including 5 compartments, namely the first compartment, the second compartment, the third compartment, the fourth compartment and the fifth compartment.
[0038] It should be noted that for the convenience of description, in Figure 1In the structure of the shown snake-shaped pipeline detector, with the centroid of the snake-shaped pipeline detector as the origin, the forward direction of the snake-shaped pipeline detector as the positive x-axis direction, a Cartesian coordinate system is defined based on the right-hand rule. The left side of the snake-shaped pipeline detector is the y-axis, and the upper side is the z-axis.
[0039] Among them, the first compartment is equipped with a sonar and a camera; the sonar is used to collect the distance between the pipe wall at each scanning angle, and the camera is used to collect the image data inside the pipeline.
[0040] The second compartment is equipped with a first thruster and a second thruster; the first thruster and the second thruster are located in the x-y plane, and the propulsion direction is parallel to the x-axis, and is used to provide power for the snake-shaped pipeline detector in the x-axis direction.
[0041] The third compartment is equipped with a third thruster and a fourth thruster. The third thruster and the fourth thruster form a 45° angle with the x-y plane, and the propulsion direction is parallel to the y-z plane, and is used to provide lateral power for the snake-shaped pipeline detector.
[0042] The fourth compartment is equipped with a nine-axis sensor, a fifth thruster, and a sixth thruster. The nine-axis sensor is used to collect the attitude of the snake-shaped pipeline detector; the nine-axis sensor includes a three-axis gyroscope, a three-axis accelerometer, and a three-axis digital compass. The three-axis gyroscope is used to measure the rotational angular velocity of the snake-shaped pipeline detector around the x, y, and z axes. The three-axis accelerometer is used to measure the acceleration of the snake-shaped pipeline detector in the x, y, and z axis directions. The three-axis digital compass is used to measure the pitch angle, roll angle, and yaw angle of the snake-shaped pipeline detector. The fifth thruster and the sixth thruster are located in the x-z plane, and the propulsion direction is parallel to the x-axis, and is used to provide power for the snake-shaped pipeline detector in the x-axis direction.
[0043] It should be noted that Figure 1 The centroid of the shown snake-shaped pipeline detector is located on the central axis of the front cylindrical compartment of the fourth compartment. Therefore, the nine-axis sensor is installed in the fourth compartment, that is, the nine-axis sensor is set at the centroid of the snake-shaped pipeline detector.
[0044] The fifth compartment is equipped with a seventh thruster. The seventh thruster is located in the x-y plane, and the propulsion direction is parallel to the z-axis, and is used to provide power for the snake-shaped pipeline detector in the z-axis direction.
[0045] In some embodiments, based on Figure 1 the shown snake-shaped pipeline detector can execute corresponding actions in response to the control instructions issued by the control terminal, including forward, backward, left and right translation, up and down, pitch, roll, and yaw. The specific control process is as Figure 2 shown.
[0046] The upper computer control terminal issues a control instruction, which includes the pitch angle offset value Δφ, the roll angle offset value Δθ, the yaw angle offset value Δψ, and the target force f.
[0047] The pitch angle offset value Δφ, the roll angle offset value Δθ, and the yaw angle offset value Δψ are added to the historical attitude Θ′ to obtain the target attitude Θ. The target attitude Θ is combined with the measured attitude Θ of the snake-shaped pipeline detector measured by the nine-axis sensor at the current moment m , and based on the PID control law, the desired angular velocity ω is obtained. Further combined with the measured angular velocity ω of the snake-shaped pipeline detector measured by the nine-axis sensor at the current moment m , the desired output angular velocity ω′ is calculated. Further, torque calculation is performed based on the desired output angular velocity ω′ to obtain the desired torque τ.
[0048] In addition, according to the minimum distance d among the multiple distances d collected by the sonar min , the target force f is corrected to obtain the desired force f′.
[0049] Furthermore, based on the desired force f′ and the desired torque τ, the propeller speeds of each thruster are calculated and based on the propeller speeds of each thruster the driving of the snake-shaped pipeline detector is controlled.
[0050] It can be understood that in the snake-shaped pipeline detector proposed in this application, based on a specific propulsion structure and sensing structure, the snake-shaped pipeline detector can achieve the motion functions of stable hovering, forward movement, yaw, roll, wall-following forward movement, and wall-following roll in the working environment of large-diameter raw water pipelines with high flow rate and high pressure.
[0051] Embodiment 2
[0052] This application embodiment provides a control method, which is applied to the snake-shaped pipeline detector as shown in Figure 1 . The flow schematic diagram of this control method is as shown in Figure 3 and includes steps S301-S307.
[0053] S301. Obtain the input control instruction.
[0054] Among them, the control instruction includes the pitch angle offset value, the roll angle offset value, the yaw angle offset value, and the target force.
[0055] As a possible implementation manner, the user can send a control instruction to the snake-shaped pipeline detector through the upper computer control terminal.
[0056] Optionally, the upper computer control terminal can be devices such as a control handle, a computer, etc. This application embodiment does not limit the specific form of the upper computer control terminal.
[0057] Exemplarily, when the host computer control terminal is a control handle, the user can output a control instruction by shaking the rocker of the control handle. Among them, the pitch angle offset value, roll angle offset value, and yaw angle offset value can be determined according to the shaking position of the remote sensor, and the target force can be determined according to the distance of the rocker shaking. When the host computer control terminal is a computer device, the user can input the corresponding data on the display interface of the computer, and the pitch angle offset value, roll angle offset value, yaw angle offset value, and target force are determined based on the filled content in the corresponding column.
[0058] S302. Determine the target pose according to the pitch angle offset value, roll angle offset value, yaw angle offset value, and the historical pose of the snake-shaped pipeline detector.
[0059] Among them, the historical pose includes the historical pitch angle, historical roll angle, and historical yaw angle.
[0060] As a possible implementation, on the basis of the historical pitch angle, adding the pitch angle offset value can obtain the target pitch angle that the required snake-shaped pipeline detector needs to reach; on the basis of the historical roll angle, adding the roll angle offset value can obtain the target roll angle that the required snake-shaped pipeline detector needs to reach; on the basis of the historical yaw angle, adding the roll angle yaw angle can obtain the target yaw angle that the required snake-shaped pipeline detector needs to reach. Based on this, the target pose including the target pitch angle, target roll angle, and target yaw angle is obtained.
[0061] In some embodiments, the expression for determining the target pose according to the pitch angle offset value, roll angle offset value, yaw angle offset value, and the historical pose of the snake-shaped pipeline detector is as follows:
[0062] Θ = Θ′ + [Δφ Δθ Δψ] T ;
[0063] [φ θ ψ] T = [φ′ θ′ ψ′] T + [Δφ Δθ Δψ] T ;
[0064] In the formula: Θ is the target pose, Θ′ is the historical pose, Δφ is the pitch angle offset value, Δθ is the roll angle offset value, Δψ is the yaw angle offset value, φ is the target pitch angle, θ is the target roll angle, ψ is the target yaw angle, φ′ is the historical pitch angle, θ′ is the historical roll angle, and ψ′ is the historical yaw angle.
[0065] S303. Obtain the measured pose of the snake-shaped pipeline detector.
[0066] Among them, the measured pose is the pose collected by the nine-axis sensor at the current moment.
[0067] S304. Determine the expected torque based on the target attitude and the measured attitude.
[0068] As a possible implementation, first calculate the input target attitude and the measured attitude based on the PID control law to determine the expected angular velocity for the snake-shaped pipeline detector to reach the target attitude from the measured attitude. Further, subtract the currently measured angular velocity of the snake-shaped pipeline detector from the calculated expected angular velocity to obtain the expected output angular velocity for the snake-shaped pipeline detector to reach the expected angular velocity from the measured angular velocity. Still further, calculate the expected torque in combination with the moment of inertia of the snake-shaped pipeline detector and the expected output angular velocity.
[0069] In some embodiments, as Figure 4 shown, step S304 specifically includes the following steps S3041 - S3043.
[0070] S3041. Determine the expected angular velocity according to the target attitude, the measured attitude, and the PID control law.
[0071] In some embodiments, the target attitude and the measured attitude can be substituted into the following expression to determine the expected angular velocity.
[0072]
[0073] e = Θ - Θ m ;
[0074] In the formula: Θ is the target attitude, Θ m is the measured attitude, ω is the expected angular velocity, K P is the proportionality coefficient, K i is the integral coefficient, K D is the differential coefficient, the variable k is used to represent the current moment, and e is the difference between the target attitude and the measured attitude.
[0075] S3042. Determine the expected output angular velocity according to the expected angular velocity and the measured angular velocity.
[0076] Among them, the measured angular velocity is the angular velocity collected by the nine-axis sensor at the current moment.
[0077] As a possible implementation, subtracting the measured angular velocity from the expected angular velocity can obtain the expected output angular velocity required to change from the measured angular velocity to the expected angular velocity.
[0078] In some embodiments, the following expression can be used to determine the expected output angular velocity based on the expected angular velocity and the measured angular velocity.
[0079] ω' = ω - ω m ;
[0080] where: ω′ is the desired output angular velocity, ω is the desired angular velocity, and ω m is the measured angular velocity.
[0081] S3043. Determine the desired torque according to the desired output angular velocity and the moment of inertia of the snake-shaped pipeline detector.
[0082] In some embodiments, the desired output angular velocity and the moment of inertia can be substituted into the following expression to determine the desired torque.
[0083] τ = J·ω′ = [τ x , τ y , τ z T ;
[0084] where: τ is the desired torque, J is the moment of inertia, ω′ is the desired output angular velocity, and τ x , τ y , τ z are the torques corresponding to the snake-shaped pipeline detector on the x, y, and z axes, respectively.
[0085] It should be noted that the moment of inertia of the snake-shaped pipeline detector can be measured by the operation and maintenance personnel through pre-simulation and experiment. The expression of the moment of inertia J of the snake-shaped pipeline detector is as follows.
[0086]
[0087] S305. Obtain multiple distances collected by the sonar.
[0088] Among them, the multiple distances are the distances between the sonar and the pipe wall in each scanning angle direction.
[0089] Exemplarily, the multiple distances collected by the sonar can be expressed as a distance array d(θ) = [d(1), d(2)... d(360)], where θ is the scanning angle and d(θ) is the distance between the sonar and the pipe wall in the scanning angle θ direction.
[0090] S306. Correct the target force according to the multiple distances to obtain the desired force.
[0091] As a possible implementation, correct the forces in the y and z axis directions in the target force according to the minimum distance among the multiple distances to obtain the desired force.
[0092] In some embodiments, correct the target force according to the multiple distances to obtain the desired force. As Figure 5 shown, it includes the following steps S3061 - S3063.
[0093] S3061. Divide multiple distances into distance arrays corresponding to four directions to obtain a first distance array, a second distance array, a third distance array, and a fourth distance array.
[0094] Among them, the distances included in the first distance array are the distances collected by the sonar when the scanning angles are 1°, 2°... 44°, and 315°, 316°... 360°. The distances included in the second distance array are the distances collected by the sonar when the scanning angles are 45°, 46°... 134°. The distances included in the third distance array are the distances collected by the sonar when the scanning angles are 135°, 136°... 224°. The distances included in the fourth distance array are the distances collected by the sonar when the scanning angles are 225°, 226°... 314°.
[0095] It should be noted that the first distance array corresponds to the positive y-axis direction, the second distance array corresponds to the positive z-axis direction, the third distance array corresponds to the negative y-axis direction, and the fourth distance array corresponds to the negative z-axis direction, as shown in the following exemplary expressions.
[0096] d y+ =[d(315), d(316)... d(360), d(1), d(2)... d(44)];
[0097] d z+ =[d(45), d(46)... d(134)];
[0098] d y- =[d(135), d(136)... d(224)];
[0099] d z- =[d(225), d(226)... d(314)].
[0100] Furthermore, based on the above four distance arrays, the minimum distances d between the snake-shaped pipeline detector and the pipe wall in the four directions can be obtained min =[d y+min , d z+min , d y-min , d z-min .
[0101] S3062. Determine the target distance array according to the direction of the target force.
[0102] Among them, the direction corresponding to the target distance array is the direction of the target force.
[0103] As a possible implementation, when the direction of the target force points to the positive y-axis direction, the target distance array is d y+; When the direction of the target force points to the negative y-axis direction, the target distance array is d y- ; When the direction of the target force points to the positive z-axis direction, the target distance array is d z+ ; When the direction of the target force points to the negative z-axis direction, the target distance array is d z- .
[0104] In some embodiments, if the direction of the target force is between the positive y-axis direction and the positive z-axis direction, the target distance array is d y+ and d z+ ; If the direction of the target force is between the positive y-axis direction and the negative z-axis direction, the target distance array is d y+ and d z- ; If the direction of the target force is between the negative y-axis direction and the positive z-axis direction, the target distance array is d y- and d z+ ; If the direction of the target force is between the negative y-axis direction and the negative z-axis direction, the target distance array is d y- and d z- .
[0105] S3063. Correct the target force according to the minimum distance in the target distance array to obtain the desired force.
[0106] As a possible implementation, determine the minimum distance in the target distance array, and then correct the target force based on this minimum distance to obtain the desired force.
[0107] When the target distance array includes two, correct the component force of the target force in the same direction based on the minimum distance in each target distance array respectively. Based on the two corrected component forces and the component force of the target force in the x-axis direction, obtain the desired force.
[0108] Exemplarily, the target force f = [f x , f y , f z T , where f x , f y , f z are the component forces of the target force in the x, y, and z-axis directions respectively. After correcting the component force f y in the y-axis direction, f' y is obtained. After correcting the component force f z in the z-axis direction, f' z is obtained. Thus, the desired force f' = [f x , f' y , f' z T .
[0109] It is understandable that the f of the target force in the x-axis direction x , which is used to control the forward and backward movement of the snake-shaped pipeline detector, is not restricted by the distance from the pipe wall, so there is no need to correct f x .
[0110] In some embodiments, according to the minimum distance in the target distance array, the expression for obtaining the desired force by correcting the target force is:
[0111]
[0112] In the formula: F′ is the desired force, F is the target force, is the first distance threshold, is the second distance threshold, the first distance threshold is greater than the second distance threshold, D is the minimum distance in the target distance array, and F d is a preset value.
[0113] It should be noted that when the distance between the snake-shaped pipeline detector and the pipe wall is greater than the first distance threshold , it means that the distance between the snake-shaped pipeline detector and the pipe wall is large, and there is no need to correct the input target force; when the distance between the snake-shaped pipeline detector and the pipe wall is less than or equal to the first distance threshold and greater than the second distance threshold , as the distance gets closer to the second distance threshold , the output force becomes smaller, showing a linear trend until the output force decreases to zero; when the distance between the snake-shaped pipeline detector and the pipe wall is less than the second distance threshold , a force opposite to the input direction is output. As the distance decreases, the magnitude of the force gradually increases until a force with a magnitude of F d and opposite to the direction of the input target force is output.
[0114] S307. Determine the propeller speed corresponding to each thruster according to the desired torque and the desired force, and control the operation of the corresponding thruster based on each propeller speed.
[0115] In some embodiments, after obtaining the desired torque and the desired force, the desired torque and the desired force can be substituted into the following expression to determine the propeller speed corresponding to each thruster.
[0116]
[0117] In the formula: M is the control efficiency matrix, τ is the desired torque, f′ is the desired force, are the propeller speeds of the first thruster, the second thruster, the third thruster, the fourth thruster, the fifth thruster, the sixth thruster, and the seventh thruster respectively, and τ x , τ y , τz They are the torques corresponding to the serpentine pipeline detector on the x, y, and z axes, respectively, f x is the force of the expected force in the x-axis direction, f′ y is the force of the expected force in the y-axis direction, f′ z is the force of the expected force in the z-axis direction.
[0118] It should be noted that the control efficiency matrix M -1 is the inverse matrix of M. The control efficiency matrix M satisfies the following relationships with the torques of each axis, forces, and the propeller speeds of each thruster.
[0119]
[0120] The control efficiency matrix M can be obtained from the positions, angles of each thruster, and the dynamic coefficients obtained through experiments. Taking the first thruster as an example, its operation is in the feathering mode. When rotating forward, its drainage direction is parallel to the reverse direction of the x-axis, the provided power direction is parallel to the positive direction of the x-axis, and the perpendicular distance from the force application point to the x-y plane is P 1,xy , and the forces and torques generated by the No. 1 thruster can be calculated as shown in the following expressions.
[0121]
[0122] In the formula: c M is the anti-torque coefficient of the propeller and needs to be determined through experiments, c t is the tensile coefficient and needs to be determined through experiments.
[0123] Furthermore, by calculating the formulas for the forces and torques of each propeller, the control efficiency matrix can be obtained, and its expression is as follows.
[0124]
[0125] In the formula: P i,xy , P i,yz , P i,xz are the distances from the i-th propeller to the x-y, y-z, and x-z planes, respectively.
[0126] Furthermore, when calculating the inverse matrix of the control efficiency matrix, the pseudo-inverse method can be used for calculation, as shown in the following expression.
[0127] M -1 = M T (M·M T ) -1 .
[0128] In summary, after determining the desired force and the desired torque, the propeller speed corresponding to each thruster can be calculated based on the control efficiency matrix, and then the corresponding propeller can be controlled to output power based on the electronic speed control method to complete a single control action.
[0129] Embodiment 3
[0130] See Figure 6 , Figure 6 which is a schematic structural diagram of the control device provided by the present application. As an implementation of the control method shown in Figure 3 , this embodiment provides a control device 60, which can be specifically applied to a snake-shaped pipeline detector. This device embodiment corresponds to the method embodiment shown in Figure 3 . The control device 60 includes an acquisition unit 61, a determination unit 62, and a processing unit 63.
[0131] The acquisition unit 61 is configured to acquire an input control instruction, where the control instruction includes a pitch angle offset value, a roll angle offset value, a yaw angle offset value, and a target force;
[0132] The determination unit 62 is configured to determine a target attitude according to the pitch angle offset value, the roll angle offset value, the yaw angle offset value, and the historical attitude of the snake-shaped pipeline detector;
[0133] The acquisition unit 61 is further configured to acquire the measured attitude of the snake-shaped pipeline detector, where the measured attitude is the attitude collected by the nine-axis sensor at the current moment;
[0134] The determination unit 62 is further configured to determine a desired torque according to the target attitude and the measured attitude;
[0135] The acquisition unit 61 is further configured to acquire a plurality of distances collected by the sonar, where the plurality of distances are the distances between the sonar and the pipe wall at each scanning angle;
[0136] The processing unit 63 is configured to correct the target force according to the plurality of distances to obtain a desired force;
[0137] The determination unit 62 is further configured to determine the propeller speed corresponding to each thruster according to the desired torque and the desired force;
[0138] The processing unit 63 is further configured to control the corresponding thruster to operate based on each propeller speed.
[0139] Embodiment 4
[0140] The embodiment of the present application further provides a computer-readable storage medium, where the computer-readable storage medium includes a stored program, and when the program runs, it controls the device where the computer-readable storage medium is located to execute the steps of the above control method.
[0141] The beneficial effects of the computer-readable storage medium of the present application are equivalent to those of the above control method, and will not be elaborated here.
[0142] The present application can be used in numerous general-purpose or special-purpose computer system environments or configurations.
[0143] For example: personal computers, server computers, handheld or portable devices, tablet devices, multiprocessor systems, microprocessor-based systems, set-top boxes, programmable consumer electronics, network PCs, minicomputers, mainframe computers, distributed computing environments including any of the above systems or devices, and so on.
[0144] The present application can be described in the general context of computer-executable instructions executed by a computer, such as program modules.
[0145] Generally, program modules include routines, programs, objects, components, data structures, etc. that perform specific tasks or implement specific abstract data types. The present application can also be practiced in a distributed computing environment, where tasks are performed by remote processing devices connected through a communication network.
[0146] In a distributed computing environment, program modules can be located in local and remote computer storage media including storage devices.
[0147] Specifically, those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through computer-readable instructions. These computer-readable instructions can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above methods. Among them, the aforementioned storage medium can be a non-volatile storage medium such as a magnetic disk, an optical disk, a read-only memory (ROM), etc., or a random access memory (RAM), etc.
[0148] It should be understood that although the steps in the flowchart of the accompanying drawings are shown in sequence according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless there is a clear indication in this article, the execution of these steps has no strict order limit and can be executed in other orders.
[0149] Moreover, at least a part of the steps in the flowchart of the accompanying drawings can include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. Their execution order is not necessarily sequential, but can be executed alternately or in turn with at least a part of other steps or sub-steps or stages of other steps.
[0150] Obviously, the embodiments described above are only a part of the embodiments of the present application, rather than all embodiments. The preferred embodiments of the present application are given in the drawings, but do not limit the patent scope of the present application. The present application can be implemented in many different forms. On the contrary, the purpose of providing these embodiments is to make the understanding of the disclosed content of the present application more thorough and comprehensive.
[0151] Although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing specific embodiments, or perform equivalent replacements for some of the technical features. Any equivalent structure made by using the content of the specification and drawings of the present application, directly or indirectly applied in other related technical fields, is equally within the scope of the patent protection of the present application.
Claims
1. A serpentine pipeline detector, characterized in that: It includes 5 cabins; The first cabin is equipped with a sonar and a camera, the second cabin is equipped with a first thruster and a second thruster, the third cabin is equipped with a third thruster and a fourth thruster, the fourth cabin is equipped with a nine-axis sensor, a fifth thruster and a sixth thruster, and the fifth cabin is equipped with a seventh thruster. The nine-axis sensor is located at the center of mass of the serpentine pipeline detector, and the nine-axis sensor includes a three-axis gyroscope, a three-axis accelerometer and a three-axis digital compass; The sonar is used to collect the distance between the sonar and the pipe wall at each scanning angle, the camera is used to collect image data in the pipe, and the nine-axis sensor is used to collect the posture of the serpentine pipe detector; Taking the center of mass as the origin and the forward direction of the serpentine pipe detector as the positive direction of the x-axis, a Cartesian coordinate system is defined based on the right-hand rule; the first propeller and the second propeller are located in the xy plane, and the propulsion direction is parallel to the x-axis, the third propeller and the fourth propeller are at an angle of 45° to the xy plane, and the propulsion direction is parallel to the yz plane, the fifth propeller and the sixth propeller are located in the xz plane, and the propulsion direction is parallel to the x-axis, and the seventh propeller is located in the xy plane, and the propulsion direction is parallel to the z-axis.
2. A control method, characterized in that: Applied to the serpentine pipeline detector according to claim 1, the method comprises: Acquire an input control instruction, wherein the control instruction includes a pitch angle offset value, a roll angle offset value, a yaw angle offset value, and a target force; Determine the target attitude according to the pitch angle offset value, the roll angle offset value, the yaw angle offset value and the historical attitude of the serpentine pipeline detector; Acquire a measurement posture of the serpentine pipeline detector, where the measurement posture is the posture collected by the nine-axis sensor at the current moment; Determining a desired torque according to the target posture and the measured posture; Acquire multiple distances collected by the sonar, where the multiple distances are the distances between the sonar and the pipe wall in each scanning angle direction; Correcting the target force according to the multiple distances to obtain a desired force; The propeller speed corresponding to each propeller is determined according to the expected torque and the expected force, and the operation of the corresponding propeller is controlled based on each propeller speed.
3. The control method according to claim 2, characterized in that: The historical posture includes a historical pitch angle, a historical roll angle, and a historical yaw angle, and the target posture includes a target pitch angle, a target roll angle, and a target yaw angle; The expression for determining the target attitude according to the pitch angle offset value, the roll angle offset value, the yaw angle offset value and the historical attitude of the serpentine pipeline detector is: Θ=Θ′+[Δφ Δθ Δψ] T ; [φ θ ψ] T =[φ′ θ′ ψ′] T +[Df Dth Ds] T ; Wherein: Θ is the target attitude, θ′ is the historical attitude, Δφ is the pitch angle offset value, Δθ is the roll angle offset value, Δψ is the yaw angle offset value, φ is the target pitch angle, θ is the target roll angle, ψ is the target yaw angle, φ′ is the historical pitch angle, θ′ is the historical roll angle, and ψ′ is the historical yaw angle.
4. The control method according to claim 2, characterized in that: The step of determining the expected torque according to the target posture and the measured posture includes: Determine the desired angular velocity according to the target posture, the measured posture and a proportional-integral-differential (PID) control law; Determine an expected output angular velocity according to the expected angular velocity and the measured angular velocity, wherein the measured angular velocity is the angular velocity collected by the nine-axis sensor at the current moment; The expected torque is determined according to the expected output angular velocity and the moment of inertia of the serpentine pipeline detector.
5. The control method according to claim 4, characterized in that: The expression for determining the desired angular velocity according to the target posture, the measured posture and the proportional integral differential PID control law is: e=Θ-Θ m ; Where: Θ is the target posture, Θ m is the measured posture, ω is the expected angular velocity, K P is the proportionality coefficient, K i is the integration coefficient, K D is the differential coefficient, the variable k is used to represent the current moment, and e is the difference between the target posture and the measured posture.
6. The control method according to claim 2, characterized in that: The step of correcting the target force according to the multiple distances to obtain the expected force includes: Dividing the multiple distances into distance arrays corresponding to four directions to obtain a first distance array, a second distance array, a third distance array and a fourth distance array; the distances included in the first distance array are distances collected by the sonar when the scanning angles are 1°, 2°...44°, and 315°, 316°...360°, the distances included in the second distance array are distances collected by the sonar when the scanning angles are 45°, 46°...134°, the distances included in the third distance array are distances collected by the sonar when the scanning angles are 135°, 136°...224°, and the distances included in the fourth distance array are distances collected by the sonar when the scanning angles are 225°, 226°...314°; Determine a target distance array according to the direction of the target force, wherein the direction corresponding to the target distance array is the direction of the target force; The target force is corrected according to the minimum distance in the target distance array to obtain the expected force.
7. The control method according to claim 6, characterized in that: The expression for obtaining the desired force by correcting the target force according to the minimum distance in the target distance array is: Where: F' is the expected force, F is the target force, is the first distance threshold, is the second distance threshold, the first distance threshold is greater than the second distance threshold, D is the minimum distance in the target distance array, and F d is a preset value.
8. The control method according to claim 2, characterized in that: The expression for determining the propeller speed corresponding to each propeller according to the expected torque and the expected force is: Where: M is the control efficiency matrix, f is the desired torque, f′ is the desired force, are the propeller speeds of the first propeller, the second propeller, the third propeller, the fourth propeller, the fifth propeller, the sixth propeller and the seventh propeller, respectively, τ x , τ y , τ z are the torques corresponding to the serpentine pipeline detector on the x, y, and z axes, respectively, and f x is the desired force in the x-axis direction, f y ' is the desired force in the y-axis direction, f z ' is the desired force in the z-axis direction.
9. A control device, characterized in that: Deployed in the serpentine pipe detector as claimed in claim 1, the control device comprises an acquisition unit, a determination unit and a processing unit; The acquisition unit is used to acquire an input control instruction, wherein the control instruction includes a pitch angle offset value, a roll angle offset value, a yaw angle offset value and a target force; The determining unit is used to determine the target posture according to the pitch angle offset value, the roll angle offset value, the yaw angle offset value and the historical posture of the serpentine pipeline detector; The acquisition unit is further used to acquire the measurement posture of the serpentine pipeline detector, where the measurement posture is the posture collected by the nine-axis sensor at the current moment; The determination unit is further configured to determine a desired torque according to the target posture and the measured posture; The acquisition unit is further used to acquire a plurality of distances collected by the sonar, wherein the plurality of distances are distances between the sonar and the pipe wall at each scanning angle; The processing unit is used to correct the target force according to the multiple distances to obtain the expected force; The determination unit is further configured to determine a propeller speed corresponding to each propeller according to the expected torque and the expected force; The processing unit is also used to control the operation of the corresponding propeller based on the rotation speed of each propeller.
10. A computer-readable storage medium, wherein instructions are stored in the computer-readable storage medium, characterized in that: When the instruction is executed on the serpentine pipeline detector, the serpentine pipeline detector executes the control method as described in any one of claims 2 to 8.