CN115639810A - Track planning and tracking method of wall-climbing robot in pipeline operation and maintenance - Google Patents
Track planning and tracking method of wall-climbing robot in pipeline operation and maintenance Download PDFInfo
- Publication number
- CN115639810A CN115639810A CN202210817315.XA CN202210817315A CN115639810A CN 115639810 A CN115639810 A CN 115639810A CN 202210817315 A CN202210817315 A CN 202210817315A CN 115639810 A CN115639810 A CN 115639810A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- pipeline
- wall
- climbing robot
- cylindrical surface
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 61
- 238000012423 maintenance Methods 0.000 title claims abstract description 18
- 239000013598 vector Substances 0.000 claims description 29
- 230000009194 climbing Effects 0.000 claims description 24
- 239000011159 matrix material Substances 0.000 claims description 20
- 230000008569 process Effects 0.000 claims description 18
- 238000013461 design Methods 0.000 claims description 13
- 238000004364 calculation method Methods 0.000 claims description 7
- 238000006243 chemical reaction Methods 0.000 claims description 5
- 238000009795 derivation Methods 0.000 claims description 3
- 239000000126 substance Substances 0.000 claims description 2
- 238000013519 translation Methods 0.000 claims description 2
- 238000001514 detection method Methods 0.000 abstract description 11
- 238000010586 diagram Methods 0.000 description 6
- 230000008901 benefit Effects 0.000 description 3
- 230000007547 defect Effects 0.000 description 3
- 230000007246 mechanism Effects 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000003466 welding Methods 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012827 research and development Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000009412 basement excavation Methods 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 238000004321 preservation Methods 0.000 description 1
- 238000001179 sorption measurement Methods 0.000 description 1
- 238000001228 spectrum Methods 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
The invention relates to a track planning and tracking method of a wall-climbing robot in pipeline operation and maintenance, which comprises the following steps: establishing a coordinate system of a cylindrical surface of the pipeline as a world coordinate system; establishing a trolley coordinate system, and determining the position relation of the trolley coordinate system relative to a world coordinate system at any moment; establishing a kinematic model of the wall-climbing robot on the inner wall of the cylindrical surface of the pipeline; performing cylindrical surface expansion on the cylindrical surface coordinate system of the pipeline to obtain a plane coordinate system; the method comprises the steps of determining the current pose of the wall-climbing robot according to the position relation of a trolley coordinate system relative to a world coordinate system, converting the target pose and the current pose into a plane coordinate system, obtaining the shortest path, converting the shortest path into a representation under the world coordinate system, obtaining a target path, and constructing a trajectory tracking controller by combining a kinematic model to realize tracking operation. Compared with the prior art, the method improves the accuracy of motion control of the wall-climbing robot in the pipeline, and has certain significance for realizing the fully-autonomous operation mode of the pipeline detection wall-climbing robot.
Description
Technical Field
The invention relates to the technical field of robot control, in particular to a track planning and tracking method of a wall-climbing robot in pipeline operation and maintenance.
Background
Because the pipeline of the gas turbine power plant runs under severe working conditions such as high temperature, high pressure and the like for a long time, the pipeline welding seam inevitably has different defects and is easy to expand into harmful factors such as cracks and the like, so that serious accidents such as leakage, pipe explosion and the like of the pipeline occur, and huge potential safety hazards are brought to the running of a power station. The detection of current pipeline welding seam need carry out ground excavation, set up the scaffold frame, demolish a large amount of preparation work in earlier stage such as heat preservation, and is consuming time and wasting force, risk height seriously influence the safe economic operation of thermal power plant, consequently development wall climbing robot carries out long-range nondestructive test to pipeline inner weld and has urgent demand. Aiming at special environmental conditions in pipeline detection tasks, various wall-climbing robot models are already available on the market. At present, international research on the wall-climbing robot mainly focuses on the aspects of the adsorption mode, the movement mechanism, the control strategy and the like of the body, and certain research results are available on key technologies such as the whole system research and development of the wall-climbing robot, the pipeline space positioning method and the trajectory tracking method. The summary is as follows:
1. the invention with the publication number of CN111412342A discloses a pipeline detection robot and a pipeline detection method aiming at the development of a whole system of a wall-climbing robot, wherein the wall-climbing robot comprises a vehicle body, a driving mechanism and a camera device, and the omnibearing camera detection of a pipeline space is realized by acquiring images of a plurality of camera devices and splicing the images; the document "design of a welding line detection robot system outside a spherical tank" (yellow seam, southeast university, 2019.) designs a wheel type wall climbing robot capable of climbing on the outer wall of the spherical tank, and designs an applicable hardware control system in combination with a working environment, so that the rationality of tank wall flaw detection of the wall climbing robot is verified.
2. In order to solve the problem of positioning of the wall-climbing robot in the pipeline space, the invention with the publication number of CN104007664A discloses a three-dimensional view simulation motion method of the wall-climbing robot in a nuclear power station, and provides a method for positioning the wall-climbing robot on the inner wall of a cylinder body at the secondary side of a steam generator in the nuclear power station by fusing data of an acceleration sensor, a distance measuring sensor, a gyroscope, a motor encoder and a camera, so that the positioning precision of the wall-climbing robot in the pipeline space is improved.
3. In order to realize the track tracking function of the mobile robot, the literature 'wall-climbing robot design and path tracking method research' (Monsantong. Harbin engineering university, 2013.) establishes kinematic models of differential wheels and omnidirectional wheel trolleys, realizes positioning in a mode of a gyroscope and a passive encoder, and realizes a navigation algorithm of straight lines and circular arc tracks on a plane based on a PID control mode; the invention with the publication number of CN113467496A discloses a design method and a control system of a two-wheeled trolley balanced track and tracking controller based on information fusion, wherein a dynamic observer is used for carrying out dimension expansion on variable information and realizing interference suppression filtering on specific frequency spectrum disturbance, and meanwhile, the trolley track tracking controller is designed by combining LQR and PID algorithms.
In summary, the wall-climbing robot system and the key technical method related to positioning and tracking can well assist field workers to complete pipeline detection tasks in simple scenes. However, semi-autonomous or even fully-autonomous control of the wall-climbing robot in the pipeline space still has some problems to be researched and optimized. Firstly, from the research and development of the whole system of the wall climbing robot, the method emphasizes on improving the detection efficiency under the operation mode of manual control of workers, and does not consider introducing an autonomous control mode; secondly, from the application of technologies such as robot positioning and trajectory tracking, the positioning method can already meet the positioning requirement of the wall climbing robot in the pipeline space, but an effective trajectory tracking method is not proposed for the wall climbing robot running in the pipeline space.
By knowing the cylindrical pipe space modeling parameters and being able to give the pipe space positions of defect points or other types of target points, it is of great importance to study the target trajectory planning and the autonomous operation control mode of the robot. The autonomous control mode of the wall climbing robot can greatly reduce the operation times of workers, improve the working efficiency and bring remarkable economic and social benefits for power enterprises.
Disclosure of Invention
The invention aims to provide a trajectory planning and tracking method of a wall climbing robot in pipeline operation and maintenance in order to overcome the defect that the prior art does not provide an effective trajectory tracking method for a wall climbing robot running in a pipeline space.
The purpose of the invention can be realized by the following technical scheme:
a track planning and tracking method of a wall-climbing robot in pipeline operation and maintenance comprises the following steps:
s1: establishing a pipeline cylindrical surface coordinate system on the pipeline and taking the pipeline cylindrical surface coordinate system as a world coordinate system; establishing a trolley coordinate system on the body of the wall-climbing robot, calculating the pose state of the wall-climbing robot in the inner wall of the pipeline through sensor data in real time in the operation process of the wall-climbing robot, and determining the position relation of the trolley coordinate system relative to a world coordinate system at any moment according to the contact position of the wall-climbing robot and the wall surface of the pipeline;
s2: establishing a kinematic model of the wall-climbing robot on the inner wall of the cylindrical surface of the pipeline;
s3: performing cylindrical surface expansion on the cylindrical surface coordinate system of the pipeline to obtain a plane coordinate system; determining the current pose of the wall-climbing robot according to the position relation of a trolley coordinate system relative to a world coordinate system, converting a preset target pose and the current pose of the wall-climbing robot from the world coordinate system into the plane coordinate system, acquiring a shortest path, and converting the shortest path into a representation under the world coordinate system to obtain a target path;
s4: and (4) according to the target path obtained in the step (S3), combining the kinematic model obtained in the step (S2) to construct a track tracking controller, and realizing the tracking operation of the wall-climbing robot on the target track on the pipeline surface.
Further, the establishment process of the coordinate system of the cylindrical surface of the pipeline specifically comprises the following steps: and determining an X axis by taking a point on the circumference of one end of the pipeline as an origin, taking the direction of the point pointing to the circle center of the circumference as a Y axis and the direction of the point vertical to the circumference as a Z axis, thereby obtaining a coordinate system of the cylindrical surface of the pipeline.
Further, the process of establishing the trolley coordinate system specifically comprises the following steps: the method comprises the steps of obtaining four contact points of a wall-climbing robot and a pipeline, taking the center of a plane formed by the four contact points as the original point of a trolley coordinate system, taking the direction perpendicular to the plane and facing the top of the trolley as the X-axis direction, and establishing the Y-axis direction and the Z-axis direction which are perpendicular to each other according to the plane to obtain the trolley coordinate system.
Furthermore, the wall-climbing robot comprises a vehicle body and four wheels, wherein the four wheels comprise two driving wheels and two driven wheels, and motor encoders and gyroscopes are arranged on the driving wheels;
the sensor data in the step S1 comprise gyroscope data and encoder data of a motor encoder, and the latest pose state reached by the wall-climbing robot after each control period is updated in real time by combining the gyroscope data and the encoder data according to the radius and the track information of the small wheel in each control period.
Further, the position relationship of the trolley coordinate system relative to the world coordinate system comprises a translation relationship and a rotation relationship, the rotation relationship is represented by a rotation matrix, and the calculation process of the rotation matrix comprises the following steps:
and (3) setting a unit orthogonal basis along each axis under a world coordinate system to form a matrix:
the unit orthogonal basis of each axis under the trolley coordinate system forms a matrix:
wherein the content of the first and second substances,
(A 1f ,A 2f ,A 3f ,A 4f ) The position coordinates of contact points of four wheels of the wall-climbing robot and the curved surface of the pipeline under a world coordinate system;
the rotation matrix R from the trolley coordinate system to the world coordinate system is calculated to obtain:
further, the process of establishing the kinematic model of the wall-climbing robot on the inner wall of the cylindrical surface of the pipeline comprises the following steps:
the known kinematic model of the wheeled differential mobile robot on a curved surface is as follows:
wherein (u, v) forms a half geodesic coordinate network on the curved surface, the u direction is the direction of a geodesic line on the curved surface, and the v direction is orthogonal to the u direction; (u) 1 ω) represent linear and angular velocity inputs, respectively; theta represents an included angle between the direction of the vehicle head and the positive direction of u; g is represented by the formula
The calculation results in that,
a unit vector in the v direction; (. Cndot.) u Representing the partial derivation of u;
establishing a semi-geodetic coordinate network on the cylindrical surface of the pipeline, acquiring the direction of a geodetic line on the cylindrical surface along the Z-axis direction of a world coordinate system, enabling the Z-axis to correspond to the u-axis direction of a semi-geodetic coordinate axis, and acquiring a v-axis orthogonal to the geodetic line along the circumferential direction at the origin of the world coordinate system to form the semi-geodetic coordinate network on the cylindrical surface of the pipeline;
the parameter G =1 in the cylindrical surface,
the obtained kinematics model of the wall-climbing robot on the inner wall of the cylindrical surface of the pipeline is as follows:
further, in step S3, the obtaining process of the target path specifically includes:
establishing a corresponding relation between a cylindrical surface coordinate system and a plane coordinate system of the pipeline: assuming that a plane coordinate system is obtained by cutting and expanding the cylindrical surface of the pipeline along a plane (YOZ), establishing a coordinate system O on the plane p (X 'Z'), making the position of the origin of the plane coordinate system the same as the position of the origin of the coordinate system of the cylindrical surface of the pipeline, and taking the Z 'axis direction as the same as the Z axis direction, wherein the X' axis direction is horizontally towards the left;
by the formula
Converting the given target pose and the current pose of the wall climbing robot from a world coordinate system into a plane coordinate system to realize the conversion of the middle points of the world coordinate system and the plane coordinate system, wherein R is the radius of the pipeline;
and calculating a parameter equation for obtaining the shortest path in a plane coordinate system, and converting the parameter equation into a pipeline cylindrical surface coordinate system to obtain a target path.
Further, the establishing process of the trajectory tracking controller comprises:
according to the kinematics model of the wall-climbing robot on the inner wall of the cylindrical surface of the pipeline, the actual pose of the wall-climbing robot is made as follows: g = [ u, v, θ =] T Then the kinematic model may be converted into
Wherein
Let the given reference trajectory be: g r ＝[u r ,v r ,θ r ] T The tracking error is expressed as:
taking secondary design function
Wherein α is an independent variable, k 1 ,k 2 Is a normal number, α r As a constant, we get an auxiliary tracking error representation:
taking z to t the derivative gives:
wherein
Indicating controller output, including in particular target linear velocity u of the vehicle body 1 Target angular velocity ω and independent variable derivatives of secondary design functions
Reference speed representing a given trajectory, including in particular a vehicle body reference linear speed u 1r And a reference angular velocity ω r ，
According to
Designing a trajectory tracking control law by an expression:
wherein K ∈ R 3×3 For positively determining the gain matrix, the resulting control law
Make it possible to
And finally realizing asymptotic convergence of the trajectory tracking error.
Further, the obtained trajectory tracking controller outputs a two-dimensional value in front, that is, a vehicle body target speed (u) 1 ω) is converted into a given angular velocity value (ω) of the servo motor in the left and right driving wheels l ,ω r ) And a direction vector, wherein the conversion process specifically comprises the following steps:
expressing the target linear velocity as a vector form of a three-dimensional space in consideration of the movement of the cart in the three-dimensional space of the cylindrical surface
By the formula
Converted into linear velocity of left and right wheels
Wherein b is the track width;
by the formula
Into angular velocities (omega) of the left and right wheels l ,ω r ) Wherein r is the radius of the wheel,
the direction vector of the tangent line at the contact point of the left wheel and the right wheel and the cylindrical surface of the pipeline is shown;
simultaneous cylindrical surface equation of pipeline, plane equation of left/right wheel obtained from space coordinate position of front and rear wheels and space circular curve equation of left/right front wheelsObtaining the space point coordinates of the actual contact points of the left wheel, the right wheel and the cylindrical surface of the pipeline by solving the equation set, and respectively expressing the space point coordinates as A 2f ' and A 1f ′；
The center of a circle of the left wheel and the right wheel is represented as A in a coordinate system of the cylindrical surface of the pipeline 2Of And A 1Of Calculating the vector A 2f ′-A 2Of Sum vector A 2f -A 2Of Angle α therebetween, vector A 1f ′-A 1Of And A 1f -A 1Of The included angle beta therebetween;
the direction vector of the tangent line at the contact point of the left wheel and the right wheel and the cylindrical surface of the pipeline is in a trolley coordinate system O m xO of (x, y, z) m The angle between the Z plane and the Z axis is alpha and beta respectively to obtain a direction vector
Expressed in the trolley coordinate system as:
the direction vector can be obtained through the rotation matrix R from the trolley coordinate system to the world coordinate system
The representation in the world coordinate system is:
further, the trajectory planning and tracking method plans a shortest path from the current position of the wall-climbing robot to the target point in the known pipeline environment, and enables the wall-climbing robot to track the reference trajectory in an asymptotic convergence manner.
Compared with the prior art, the invention has the following advantages:
(1) The invention utilizes the kinematics model of the wheel type differential robot on the curved surface to obtain the kinematics model of the wall-climbing robot on the cylindrical surface of the pipeline, and improves the accuracy of the motion control of the wall-climbing robot in the pipeline by combining the geometrical characteristic of the actual contact of the wheels and the wall surface of the pipeline;
(2) The invention realizes the movement track planning of the shortest distance between two points in the pipeline space based on the simple geometric corresponding relation between the cylindrical surface and the plane;
(3) Compared with the existing system, the track tracking controller for the autonomous operation of the wall-climbing robot on the cylindrical surface of the pipeline provided by the invention can reduce the labor intensity of workers, and has certain significance for realizing the fully autonomous operation mode of the pipeline detection wall-climbing robot.
Drawings
Fig. 1 is a schematic flow chart of a trajectory planning and tracking method of a wall-climbing robot in pipeline operation and maintenance provided in an embodiment of the present invention;
fig. 2 is a schematic structural view of a wall-climbing robot provided in an embodiment of the present invention;
FIG. 3 is a schematic view of a pipe provided in an embodiment of the present invention;
fig. 4 is a schematic view illustrating an operation state in a pipeline of the wall-climbing robot provided in the embodiment of the present invention;
FIG. 5 is a schematic view of a u-direction tracking curve according to an embodiment of the present invention;
FIG. 6 is a schematic view of a v-direction tracking curve provided in an embodiment of the present invention;
FIG. 7 is a schematic diagram of a corner tracking curve provided in an embodiment of the present invention;
FIG. 8 is a schematic diagram of a position tracking error curve provided in an embodiment of the present invention;
FIG. 9 is a schematic diagram of a velocity tracking error curve provided in an embodiment of the present invention;
fig. 10 is a schematic diagram of a pipeline surface trajectory tracking provided in an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention. The components of embodiments of the present invention generally described and illustrated in the figures herein may be arranged and designed in a wide variety of different configurations.
Thus, the following detailed description of the embodiments of the present invention, presented in the figures, is not intended to limit the scope of the invention, as claimed, but is merely representative of selected embodiments of the invention. All other embodiments, which can be obtained by a person skilled in the art based on the embodiments of the present invention without any inventive step, shall fall within the scope of protection of the present invention.
It should be noted that: like reference numbers and letters refer to like items in the following figures, and thus, once an item is defined in one figure, it need not be further defined and explained in subsequent figures.
Example 1
The embodiment provides a track planning and tracking method of a wall-climbing robot in pipeline operation and maintenance, and the method flow is shown in fig. 1. Fig. 2 is a schematic structural view of a wall-climbing robot according to an embodiment of the present invention, the wall-climbing robot includes a vehicle body and four wheels, wherein the front two wheels are driving wheels, i.e., driving wheels, and the rear two wheels are driven wheels, and the wall-climbing robot adopts a differential driving manner, and is provided with a motor encoder and a gyroscope of a driving mechanism. Fig. 3 is a schematic diagram of a pipeline provided in an embodiment of the present invention. Referring to fig. 4, the wall climbing robot is attached to a cylindrical surface of an inner wall of a pipe while operating.
The specific implementation process comprises the following steps:
s1: establishing a coordinate system on a cylindrical surface of a pipeline as a world coordinate system, establishing the coordinate system on a vehicle body of the wall-climbing robot as a trolley coordinate system, acquiring sensor data in the vehicle body in real time in the running process of the wall-climbing robot, calculating the pose state of the wall-climbing robot in the inner wall of the pipeline in real time, and determining the position relation of the trolley coordinate system relative to the world coordinate system at any moment according to the calculation of the contact position of wheels of the trolley and the wall surface of the pipeline;
s2: according to the world coordinate system and the trolley coordinate system established in the S1, a kinematic model of the wall climbing robot on the curved surface of the pipeline is established, the curved surface of the pipeline is a cylindrical surface with known parameters, and according to the kinematic model of the left and right wheel differential trolleys on the curved surface and the cylindrical surface parameters, the kinematic model of the wall climbing robot on the cylindrical surface can be obtained;
s3: establishing a relation between a pipeline cylindrical surface coordinate system and a plane coordinate system after the cylindrical surface is unfolded, obtaining the current pose of the wall-climbing robot according to the position relation of the trolley coordinate system relative to the world coordinate system obtained in the S1, converting the given target pose and the current pose of the wall-climbing robot into the plane coordinate system from the world coordinate system, obtaining a shortest path in the plane coordinate system through a straight line between two points, and converting a path expression into the representation under the world coordinate system to obtain a target path with the shortest distance between the two points in the pipeline space;
s4: and (4) designing a track tracking controller according to the pipeline space target path generated in the step (3) and by combining the cylindrical surface wall climbing robot kinematic model obtained in the step (2), so that the wall climbing robot can track and run the target track on the pipeline surface.
More specifically, the method for establishing the coordinate system in step S1 is as follows: aiming at the cylindrical surface of the pipeline, taking a certain point on the circumference of a certain end of the pipeline as an origin, taking the direction of the point pointing to the center of the circumference as a Y axis, taking the direction of the point vertical to the circumference, namely the direction along the height of the cylindrical body of the pipeline as a Z axis, and finally determining the direction of the X axis by a right-hand rule to obtain a coordinate system O of the cylindrical surface of the pipeline f (X, Y, Z); aiming at a wall-climbing robot body, firstly four contact points (A) of the outer edge of a wheel and a curved surface when the wall-climbing robot is placed at the inlet of a pipeline are obtained 1m ,A 2m ,A 3m ,A 4m ) The contact points represent the positions of four wheels in a trolley coordinate system, wherein 1, 2, 3 and 4 respectively represent the wheels at four positions of right front, left rear and right rear, then the center of a plane formed by the four contact points is taken as the origin of the trolley coordinate system, the upward direction vertical to the plane is taken as the x-axis direction, the direction that the No. 2 wheel points to the No. 1 wheel is taken as the y-axis direction, and the direction that the No. 3 wheel points to the No. 2 wheel is taken as the z-axis direction, so as to obtain a trolley coordinate system O m (x,y,z)。
And S1, acquiring sensor data in the vehicle body in real time, wherein the sensor data comprises gyroscope data and encoder data of left and right wheel driving motors, and updating the latest pose state reached by the motion of the wall climbing robot after each control period in real time by combining the gyroscope data and the encoder data of the left and right wheel driving motors according to the radius and the wheel track information of the small wheels in each control period.
More specifically, in step S1, the positional relationship of the cart coordinate system with respect to the world coordinate system includes a translational relationship and a rotational relationship, where the rotational relationship from the cart coordinate system to the world coordinate system is represented by a rotation matrix, and the calculation method includes:
s11: let the unit orthogonal basis along XYZ axis under world coordinate system constitute matrix:
s12: the unit orthogonal basis along the xyz axis under the trolley coordinate system forms a matrix:
wherein:
s13: the rotation matrix R from the trolley coordinate system to the world coordinate system is calculated to obtain:
more specifically, the method for establishing the kinematics model of the wall-climbing robot on the cylindrical surface in the step S2 comprises the following steps:
s21: the known kinematic model of the wheeled differential mobile robot on a curved surface is as follows:
wherein (u, v) forms one half of a curved surfaceThe u direction is the direction of a geodesic line on the curved surface, and the v direction is orthogonal to the u direction; (u) 1 ω) represent linear and angular velocity inputs, respectively; theta represents an included angle between the direction of the vehicle head and the positive direction of u; g is represented by the formula
The result of the calculation is that,
a unit vector in the v direction; (. Cndot.) u Represents the partial derivation of u.
S22: establishing a semi-geodetic coordinate net on the cylindrical surface of the pipeline, acquiring the direction of a geodetic line on the cylindrical surface along the Z-axis direction of a world coordinate system, enabling the Z-axis to correspond to the u-axis direction of a semi-geodetic coordinate axis, and acquiring a v-axis orthogonal to the geodetic line along the circumferential direction at the origin of the world coordinate system to form the semi-geodetic coordinate net on the cylindrical surface of the pipeline;
s23: the parameter G =1 in the cylindrical surface,
the obtained kinematics model of the wall-climbing robot on the inner wall of the cylindrical surface of the pipeline is as follows:
more specifically, the method for planning the shortest path between two points in the pipeline space in step S3 includes:
s31: establishing a relation between a pipeline cylindrical surface coordinate system and a plane coordinate system after the cylindrical surface is unfolded: the plane is assumed to be obtained by cutting and unfolding the cylindrical surface of the pipeline along the plane (YOZ), and a coordinate system O is established on the plane p (X 'Z'), making the position of the origin of the plane coordinate system the same as the position of the origin of the coordinate system of the cylindrical surface of the pipeline, and taking the Z 'axis direction as the same as the Z axis direction, wherein the X' axis direction is horizontally towards the left;
s32: by the formula
Converting the given target pose and the current pose of the wall climbing robot from a world coordinate system into a plane coordinate system to realize the conversion of the middle points of the world coordinate system and the plane coordinate system, wherein R is the radius of the pipeline;
s33: and calculating a parameter equation of a straight line between two points in the plane coordinate system, and converting the parameter equation into a pipeline cylindrical surface coordinate system through a formula in S32 to obtain a target path.
More specifically, the method for designing the trajectory tracking controller of the wall-climbing robot running on the pipeline surface in step S4 is as follows:
s41: according to the kinematics model of the wall climbing robot on the inner wall of the cylindrical surface of the pipeline, the actual pose of the wall climbing robot is as follows: g = [ u, v, θ =] T Then the kinematic model may be converted into
Wherein
S42: let the given reference trajectory be: g is a radical of formula r ＝[u r ,v r ,θ r ] T The tracking error can be expressed as:
s43: taking secondary design function
Wherein α is an independent variable, k 1 ,k 2 Is a normal number, α r As a constant, we get an auxiliary tracking error representation:
taking z to t the derivative gives:
wherein
Indicating controller output, including in particular target linear velocity u of the vehicle body 1 Target angular velocity ω and independent variable derivative of secondary design function
Reference speed representing a given trajectory, including in particular a vehicle body reference linear speed u 1r And a reference angular velocity ω r ，
S44: according to
The expression can design a track tracking control law:
wherein K ∈ R 3×3 For positively determining the gain matrix, the resulting control law
Make it possible to
And finally realizing asymptotic convergence of the trajectory tracking error.
More specifically, the stepsController output obtained in step S44
Including the target speed (u) of the vehicle body 1 ω) and the derivative of the argument of the secondary design function f (α)
In which the target speed of the vehicle body is converted into a given angular velocity value (omega) of the servo motor for the left and right drive wheels l ,ω r ) The method comprises the following steps:
s51: considering the movement of the trolley in the three-dimensional space of the cylindrical surface, the target linear speed is expressed in the form of a vector of the three-dimensional space
By the formula
Converted into linear velocity of left and right wheels
Wherein b is the track width;
s52: by the formula
Into angular velocities of the left and right wheels (omega) l ,ω r ) Wherein r is the radius of the wheel,
the direction vector of the tangent line at the contact point of the left wheel and the right wheel and the cylindrical surface of the pipeline is shown;
s53: the simultaneous pipe cylindrical surface equation, the left/right wheel plane equation obtained from the space coordinate positions of the front and rear wheels and the space circular curve equation of the left/right front wheels are used for obtaining an equation set, and the space point coordinates of the actual contact points of the left and right wheels and the pipe cylindrical surface are obtained by solving and are respectively expressed as A 2f ' and A 1f ′；
S54: setting the representation of the center of a circle of the left and right wheels in the coordinate system of the cylindrical surface of the pipelineIs A 2Of And A 1Of Can calculate vector A 2f ′-A 2Of Sum vector A 2f -A 2Of Angle α therebetween, vector A 1f ′-A 1Of And A 1f -A 1Of The included angle beta therebetween;
s55: the direction vector of the tangent line at the contact point of the left wheel and the right wheel and the cylindrical surface of the pipeline is in a trolley coordinate system O m xO of (x, y, z) m The angle between the z-plane and the z-axis is alpha and beta, respectively, to obtain the direction vector
Expressed in the trolley coordinate system as:
s56: the direction vector can be obtained through the rotation matrix R from the trolley coordinate system to the world coordinate system
The representation in the world coordinate system is:
the obtained pipeline cylindrical surface kinematics model of the wall-climbing robot is used for planning the pipeline surface path of the wall-climbing robot and carrying out track following simulation tests by setting a group of starting points and end points on the pipeline so as to illustrate the effectiveness of the invention:
1) Arranging a pipeline as a section of vertical cylindrical surface with the radius of 2m and the height of 10 m;
2) In the coordinate system of the cylindrical surface of the pipeline, the coordinates of the starting point of the path are set as follows: a = [0;4;0] and end point coordinates a = [0;0;10];
3) Arranging the wall climbing robot at the path starting point coordinate, wherein the direction of the vehicle head is vertical upwards;
in this embodiment, specific parameters of the trajectory tracking control method provided by the present invention are set as shown in table 1.
TABLE 1
Fig. 5 to fig. 10 respectively show a position tracking curve, a position tracking error curve, a speed tracking error curve, and a pipe surface trajectory tracking effect diagram of the wall-climbing robot provided by the present invention for realizing trajectory planning and tracking in a pipe space under the parameters set in this embodiment. The experimental results show that the method for realizing the trajectory planning and tracking of the wall-climbing robot in the pipeline space can well realize the path planning in the pipeline space and realize the asymptotic tracking of the wall-climbing robot on the reference trajectory.
The foregoing detailed description of the preferred embodiments of the invention has been presented. It should be understood that numerous modifications and variations could be devised by those skilled in the art in light of the present teachings without departing from the inventive concepts. Therefore, the technical solutions available to those skilled in the art through logic analysis, reasoning and limited experiments based on the prior art according to the concept of the present invention should be within the scope of protection defined by the claims.
Claims (10)
1. A track planning and tracking method of a wall-climbing robot in pipeline operation and maintenance is characterized by comprising the following steps:
s1: establishing a pipeline cylindrical surface coordinate system on the pipeline and taking the pipeline cylindrical surface coordinate system as a world coordinate system; establishing a trolley coordinate system on a vehicle body of the wall-climbing robot, calculating the pose state of the wall-climbing robot in the inner wall of the pipeline through sensor data in real time in the operation process of the wall-climbing robot, and determining the position relation of the trolley coordinate system relative to a world coordinate system at any moment according to the contact position of the wall-climbing robot and the wall surface of the pipeline;
s2: establishing a kinematic model of the wall-climbing robot on the inner wall of the cylindrical surface of the pipeline;
s3: performing cylindrical surface expansion on a cylindrical surface coordinate system of the pipeline to obtain a plane coordinate system; determining the current pose of the wall-climbing robot according to the position relation of a trolley coordinate system relative to a world coordinate system, converting a preset target pose and the current pose of the wall-climbing robot from the world coordinate system into the plane coordinate system, acquiring a shortest path, and converting the shortest path into a representation under the world coordinate system to obtain a target path;
s4: and (4) according to the target path obtained in the step (S3) and in combination with the kinematics model obtained in the step (S2), constructing a trajectory tracking controller to realize the tracking operation of the wall-climbing robot on the target trajectory on the pipeline surface.
2. The method for planning and tracking the track of the wall-climbing robot in the operation and maintenance of the pipeline according to claim 1, wherein the process for establishing the coordinate system of the cylindrical surface of the pipeline specifically comprises the following steps: and determining an X axis by taking a point on the circumference of one end of the pipeline as an origin, taking the direction of the point pointing to the circle center of the circumference as a Y axis and the direction of the point vertical to the circumference as a Z axis, thereby obtaining a coordinate system of the cylindrical surface of the pipeline.
3. The method for planning and tracking the track of the wall-climbing robot in the operation and maintenance of the pipeline according to claim 1, wherein the process for establishing the trolley coordinate system specifically comprises the following steps: the method comprises the steps of obtaining four contact points of a wall-climbing robot and a pipeline, taking the center of a plane formed by the four contact points as the origin of a trolley coordinate system, taking the direction perpendicular to the plane and facing the top of the trolley as the X-axis direction, and establishing the Y-axis direction and the Z-axis direction which are perpendicular to each other according to the plane to obtain the trolley coordinate system.
4. The method for planning and tracking the track of the wall-climbing robot in the operation and maintenance of the pipeline according to claim 1, wherein the wall-climbing robot comprises a vehicle body and four wheels, the four wheels comprise two driving wheels and two driven wheels, and the driving wheels are provided with a motor encoder and a gyroscope;
the sensor data in the step S1 comprise gyroscope data and encoder data of a motor encoder, and the latest pose state reached by the motion of the wall-climbing robot after each control period is updated in real time by combining the gyroscope data and the encoder data according to the radius and the wheel track information of the small wheels in each control period.
5. The trajectory planning and tracking method of the wall-climbing robot in the pipeline operation and maintenance according to claim 1, wherein the position relationship of the trolley coordinate system relative to the world coordinate system comprises a translation relationship and a rotation relationship, the rotation relationship is represented by a rotation matrix, and the calculation process of the rotation matrix comprises:
and (3) setting unit orthogonal bases along each axis under a world coordinate system to form a matrix:
the unit orthogonal basis of each axis under the trolley coordinate system forms a matrix:
wherein the content of the first and second substances,
(A 1f ,A 2f ,A 3f ,A 4f ) The position coordinates of contact points of four wheels of the wall-climbing robot and the curved surface of the pipeline under a world coordinate system;
the rotation matrix R from the trolley coordinate system to the world coordinate system is calculated to obtain:
6. the method for planning and tracking the track of the wall-climbing robot in the operation and maintenance of the pipeline according to claim 1, wherein the process of establishing the kinematic model of the wall-climbing robot on the inner wall of the cylindrical surface of the pipeline comprises the following steps:
the known kinematic model of the wheeled differential mobile robot on a curved surface is as follows:
wherein (u, v) forms a half geodetic coordinate network on the curved surface, the u direction is the direction of a geodetic line on the curved surface, and the v direction is orthogonal to the u direction; (u) 1 ω) represents linear and angular velocity inputs, respectively; theta represents an included angle between the direction of the vehicle head and the positive direction of u; g is represented by the formula
The result of the calculation is that,
a unit vector in the v direction; (.) u Representing the partial derivation of u;
establishing a semi-geodetic coordinate network on the cylindrical surface of the pipeline, acquiring the direction of a geodetic line on the cylindrical surface along the Z-axis direction of a world coordinate system, enabling the Z-axis to correspond to the u-axis direction of a semi-geodetic coordinate axis, and acquiring a v-axis orthogonal to the geodetic line along the circumferential direction at the origin of the world coordinate system to form the semi-geodetic coordinate network on the cylindrical surface of the pipeline;
parameters in cylindrical surface
The obtained kinematics model of the wall-climbing robot on the inner wall of the cylindrical surface of the pipeline is as follows:
7. the trajectory planning and tracking method of the wall-climbing robot in the pipeline operation and maintenance according to claim 1, wherein in step S3, the obtaining process of the target path specifically comprises:
establishing a corresponding relation between a cylindrical surface coordinate system and a plane coordinate system of the pipeline: assuming that the plane coordinate system is cut along the plane (YOZ) from the cylindrical surface of the pipeThen, the coordinate system O is established on the plane p (X 'Z'), making the position of the origin of the plane coordinate system the same as the position of the origin of the coordinate system of the cylindrical surface of the pipeline, and taking the Z 'axis direction as the same as the Z axis direction, wherein the X' axis direction is horizontally towards the left;
by the formula
Converting the given target pose and the current pose of the wall climbing robot from a world coordinate system into a plane coordinate system to realize the conversion of the middle points of the world coordinate system and the plane coordinate system, wherein R is the radius of the pipeline;
and calculating a parameter equation for obtaining the shortest path in a plane coordinate system, and converting the parameter equation into a pipeline cylindrical surface coordinate system to obtain a target path.
8. The trajectory planning and tracking method of the wall-climbing robot in the pipeline operation and maintenance according to claim 1, wherein the establishment process of the trajectory tracking controller comprises the following steps:
according to the kinematics model of the wall climbing robot on the inner wall of the cylindrical surface of the pipeline, the actual pose of the wall climbing robot is as follows: g = [ u, v, θ =] T Then the kinematic model may be converted into
Wherein
Let the given reference trajectory be: g r ＝[u r ,v r ,θ r ] T The tracking error is expressed as:
taking secondary design function
Wherein α is an independent variable, k 1 ,k 2 Is a normal number, α r As a constant, we get an auxiliary tracking error representation:
taking z as a derivative of t yields:
wherein
Indicating controller output, including in particular target linear velocity u of the vehicle body 1 Target angular velocity ω and independent variable derivative of secondary design function
The reference speed representing a given trajectory, including in particular a vehicle body reference linear speed u 1r And a reference angular velocity ω r ，
According to
Designing a trajectory tracking control law by an expression:
wherein K ∈ R 3×3 For positively determining the gain matrix, the resulting control law
Make it
And finally, realizing asymptotic convergence of the track tracking error.
9. The method for planning and tracking the track of the wall-climbing robot in the operation and maintenance of the pipeline as claimed in claim 8, wherein the obtained track tracking controller outputs the previous two-dimensional value, namely the target speed (u) of the vehicle body 1 ω) is converted into a given angular velocity value (ω) of the servo motor in the left and right driving wheels l ,ω r ) And a direction vector, wherein the conversion process specifically comprises the following steps:
considering the movement of the trolley in the three-dimensional space of the cylindrical surface, the target linear speed is expressed in the form of a vector of the three-dimensional space
By the formula
Converted into linear velocity of left and right wheels
Wherein b is the track width;
by the formula
Into angular velocities (omega) of the left and right wheels l ,ω r ) Wherein r is the radius of the wheel,
the direction vector of the tangent line at the contact point of the left wheel and the right wheel and the cylindrical surface of the pipeline is shown;
the equation set is obtained by combining the cylindrical surface equation of the pipeline, the plane equation of the left/right wheels obtained by the space coordinate positions of the front and rear wheels and the space circular curve equation of the left/right front wheels, and the space point coordinates of the actual contact points of the left and right wheels and the cylindrical surface of the pipeline are obtained by solving and are respectively expressed as A 2f ' and A 1f ′；
The center of a circle of the left wheel and the right wheel is represented as A in a coordinate system of a cylindrical surface of the pipeline 2Of And A 1Of Calculating the vector A 2f ′-A 2Of Sum vector A 2f -A 2Of Angle α therebetween, vector A 1f ′-A 1Of And A 1f -A 1Of The included angle beta therebetween;
the direction vector of the tangent line at the contact point of the left wheel and the right wheel and the cylindrical surface of the pipeline is in a trolley coordinate system O m xO of (x, y, z) m The angle between the Z plane and the Z axis is alpha and beta respectively to obtain direction vectors
Expressed in the trolley coordinate system as:
the direction vector can be obtained through the rotation matrix R from the trolley coordinate system to the world coordinate system
The representation in the world coordinate system is:
10. the trajectory planning and tracking method for the wall-climbing robot in the pipeline operation and maintenance according to claim 1, wherein the trajectory planning and tracking method plans a shortest path from the current position of the wall-climbing robot to the target point in the known pipeline environment, and makes the tracking of the reference trajectory by the wall-climbing robot converge asymptotically.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210817315.XA CN115639810A (en) | 2022-07-12 | 2022-07-12 | Track planning and tracking method of wall-climbing robot in pipeline operation and maintenance |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210817315.XA CN115639810A (en) | 2022-07-12 | 2022-07-12 | Track planning and tracking method of wall-climbing robot in pipeline operation and maintenance |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115639810A true CN115639810A (en) | 2023-01-24 |
Family
ID=84939962
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210817315.XA Pending CN115639810A (en) | 2022-07-12 | 2022-07-12 | Track planning and tracking method of wall-climbing robot in pipeline operation and maintenance |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115639810A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116069040A (en) * | 2023-03-06 | 2023-05-05 | 之江实验室 | Path planning method and device for wall climbing robot constrained by curved surface of pipeline |
| CN116605326A (en) * | 2023-05-16 | 2023-08-18 | 南通唐人电子科技有限公司 | Wall climbing robot controlled by cable mechanism and control method thereof |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20010009585A (en) * | 1999-07-12 | 2001-02-05 | 김헌출 | Method of producing motion planning for troweling robot |
| CN103984344A (en) * | 2014-05-20 | 2014-08-13 | 中科华核电技术研究院有限公司 | Wall-climbing robot path planning method for secondary side of nuclear power plant steam generator |
| CN112558622A (en) * | 2020-11-30 | 2021-03-26 | 华南理工大学 | Control method of foot end trajectory tracking controller of wall-climbing hexapod robot |
| CN113253745A (en) * | 2021-03-23 | 2021-08-13 | 华南理工大学 | Circumferential motion pose planning method for four-footed pipeline robot |
-
2022
- 2022-07-12 CN CN202210817315.XA patent/CN115639810A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20010009585A (en) * | 1999-07-12 | 2001-02-05 | 김헌출 | Method of producing motion planning for troweling robot |
| CN103984344A (en) * | 2014-05-20 | 2014-08-13 | 中科华核电技术研究院有限公司 | Wall-climbing robot path planning method for secondary side of nuclear power plant steam generator |
| CN112558622A (en) * | 2020-11-30 | 2021-03-26 | 华南理工大学 | Control method of foot end trajectory tracking controller of wall-climbing hexapod robot |
| CN113253745A (en) * | 2021-03-23 | 2021-08-13 | 华南理工大学 | Circumferential motion pose planning method for four-footed pipeline robot |
Non-Patent Citations (1)
| Title |
|---|
| 葛浩然: "爬壁机器人机电耦合动力学与轨迹跟踪控制研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 1, 15 January 2022 (2022-01-15), pages 140 - 1081 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116069040A (en) * | 2023-03-06 | 2023-05-05 | 之江实验室 | Path planning method and device for wall climbing robot constrained by curved surface of pipeline |
| CN116605326A (en) * | 2023-05-16 | 2023-08-18 | 南通唐人电子科技有限公司 | Wall climbing robot controlled by cable mechanism and control method thereof |
| CN116605326B (en) * | 2023-05-16 | 2023-11-07 | 南通唐人电子科技有限公司 | Control method of wall climbing robot controlled by cable mechanism |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115639810A (en) | Track planning and tracking method of wall-climbing robot in pipeline operation and maintenance | |
| CN110703762B (en) | Hybrid path planning method for unmanned surface vehicle in complex environment | |
| CN104476550B (en) | The method of operating of all-hydraulic autonomous mechanical arm | |
| CN105081524B (en) | In welding process, the online dynamic programming of track follows the tracks of collaborative control method with welding bead | |
| CN108481324B (en) | Inverse solution engineering of eight-axis multifunctional mechanical arm and collision detection algorithm thereof | |
| CN105751200A (en) | Action method of fully hydraulic autonomous moving mechanical arm | |
| Korayem et al. | Trajectory optimization of nonholonomic mobile manipulators departing to a moving target amidst moving obstacles | |
| Liu et al. | Robot path planning with two-axis positioner for non-ideal sphere-pipe joint welding based on laser scanning | |
| Yang et al. | Mobile robot motion control and autonomous navigation in GPS-denied outdoor environments using 3D laser scanning | |
| Liu et al. | Welding seam recognition and tracking for a novel mobile welding robot based on multi-layer sensing strategy | |
| Guan | Research on the application of robot welding technology in modern architecture | |
| Cimurs et al. | Time-optimized 3D path smoothing with kinematic constraints | |
| Nagai et al. | Three-dimensional obstacle avoidance online path-planning method for autonomous mobile overhead crane | |
| Lakhal et al. | Control of a hyper-redundant robot for quality inspection in additive manufacturing for construction | |
| Vanegas et al. | Smooth 3D path planning for non-holonomic UAVs | |
| Xu et al. | Advances in multi-robotic welding techniques: A review | |
| Mu et al. | Intelligent demolition robot: Structural statics, collision detection, and dynamic control | |
| Yoo et al. | A study on a mobile platform-manipulator welding system for horizontal fillet joints | |
| Che et al. | A wall-following navigation method for autonomous driving based on Lidar in tunnel scenes | |
| Li et al. | Spatial positioning robotic system for autonomous inspection of LPG tanks | |
| CN114347017B (en) | Curved surface motion control method of adsorption type mobile processing robot based on plane projection | |
| Vosniakos et al. | Development of robotic welding stations for pressure vessels: interactive digital manufacturing approaches | |
| CN104776968A (en) | Calculation method for PD (penetration depth) of multi-joint model | |
| Yin et al. | Welding seam detection and tracking based on laser vision for robotic arc welding | |
| Lee et al. | Computer Vision-Based Robotic Welding for Construction of Nuclear Power Plants |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| TA01 | Transfer of patent application right | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20230619 Address after: 200942 No. 270, Sheng Shi Road, Baoshan District, Shanghai Applicant after: HUANENG (SHANGHAI) ELECTRIC POWER MAINTENANCE CO.,LTD. Applicant after: Shanghai University of Electric Power Address before: 201306 No. 1851, Hucheng Ring Road, Pudong New Area, Shanghai Applicant before: Shanghai University of Electric Power |