Trajectory planning and tracking method for wall-climbing robots in pipeline operation and maintenance

Abstract

The present invention relates to a trajectory planning and tracking method for a wall-climbing robot in pipeline operation and maintenance, including: establishing a pipeline cylindrical surface coordinate system as a world coordinate system; establishing a trolley coordinate system, determining the positional relationship of the trolley coordinate system relative to the world coordinate system at any time; establishing a kinematic model of the wall-climbing robot on the inner wall of the pipeline cylindrical surface; performing cylindrical surface expansion on the pipeline cylindrical surface coordinate system to obtain a plane coordinate system; determining the current posture of the wall-climbing robot according to the positional relationship of the trolley coordinate system relative to the world coordinate system, converting the target posture and the current posture into the plane coordinate system, obtaining the shortest path, and converting them to the world coordinate system to obtain the target path, and combining the kinematic model to construct a trajectory tracking controller to realize tracking operation. Compared with the prior art, the present invention improves the accuracy of the 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

[0001] The present invention relates to the field of robot control technology, and in particular to a trajectory planning and tracking method of a wall-climbing robot in pipeline operation and maintenance. Background Art

[0002] Since the pipelines of gas turbine power plants operate under harsh conditions such as high temperature and high pressure for a long time, the pipeline welds inevitably have various defects and are easy to expand into hazardous factors such as cracks, which lead to serious accidents such as pipeline leakage and pipe burst, posing huge safety hazards to the operation of power plants. At present, the detection of pipeline welds requires a lot of preliminary preparation work such as ground excavation, scaffolding construction, and removal of insulation layers. It is time-consuming, labor-intensive, and high-risk, which seriously affects the safe and economical operation of thermal power plants. Therefore, there is an urgent need to develop wall-climbing robots to perform remote non-destructive testing of welds in pipelines. In response to the special environmental conditions in pipeline inspection tasks, a variety of wall-climbing robot prototypes have appeared on the market. At present, international research on wall-climbing robots mainly focuses on the adsorption method, motion mechanism, control strategy, etc. of the main body. There have been certain research results in key technologies such as the development of wall-climbing robot whole system, pipeline spatial positioning and trajectory tracking methods. The summary is as follows:

[0003] 1. Regarding the development of the wall-climbing robot system, the invention with publication number CN111412342A discloses a pipeline inspection robot and a pipeline inspection method. The wall-climbing robot includes a body, a driving mechanism and a camera device. By acquiring images from multiple cameras and performing image stitching, it realizes all-round camera inspection of the pipeline space; the document "Design of a Spherical Tank External Weld Detection Robot System" (Huang Feng. Southeast University, 2019.) designs a wheeled wall-climbing robot that can crawl on the outer wall of a spherical tank, and designs a suitable hardware control system based on the working environment, verifying the rationality of the wall-climbing robot's tank wall flaw detection.

[0004] 2. In order to solve the positioning problem of the wall-climbing robot in the pipeline space, the invention with publication number CN104007664A discloses a three-dimensional visual simulation motion method for a nuclear power plant wall-climbing robot, and proposes a method of integrating acceleration sensors, ranging sensors, gyroscopes, motor encoders and camera data to realize the positioning of the wall-climbing robot on the inner wall of the secondary side cylinder of the nuclear power plant steam generator, thereby improving the positioning accuracy of the wall-climbing robot in the pipeline space.

[0005] 3. In order to realize the trajectory tracking function of the mobile robot, the document "Design of Wall-climbing Robot and Research on Path Tracking Method" (Meng Xiangyu. Harbin Engineering University, 2013.) established the kinematic model of differential wheel and omnidirectional wheel car, realized positioning by means of gyroscope plus passive encoder, and realized the navigation algorithm of straight line and circular arc trajectory on the plane based on PID control method; the invention with publication number CN113467496A discloses a design method and control system of a two-wheeled car balance trajectory and tracking controller based on information fusion, in which a dynamic observer is used to expand the dimension of variable information and realize the suppression filtering of specific spectral disturbances, and the car trajectory tracking controller is designed by combining LQR and PID algorithms.

[0006] In general, the above-mentioned wall-climbing robot system and key technical methods related to positioning and tracking can better assist on-site staff to complete pipeline inspection tasks in simple scenarios. However, there are still some problems to be studied and optimized for the semi-autonomous or even fully autonomous control of wall-climbing robots in pipeline space. First, from the perspective of the research and development of the whole system of the wall-climbing robot, the above-mentioned method focuses on improving the detection efficiency under the manual control operation mode of the staff, and does not consider the introduction of autonomous control methods; secondly, from the perspective of the application of technologies such as robot positioning and trajectory tracking, the above-mentioned positioning method can already meet the positioning requirements of the wall-climbing robot in the pipeline space, but no effective trajectory tracking method is proposed for the wall-climbing robot operating in the pipeline space.

[0007] It is of great significance to study the target trajectory planning and the autonomous operation control mode of the robot by knowing the spatial modeling parameters of the cylindrical pipeline and being able to give the spatial position of the defect point or other types of target points in the pipeline. The autonomous control mode of the wall-climbing robot can not only greatly reduce the number of operations of the staff and improve work efficiency, but also bring significant economic and social benefits to power companies. Summary of the invention

[0008] The purpose of the present invention is to overcome the defect that the above-mentioned prior art has not proposed an effective trajectory tracking method for a wall-climbing robot operating in a pipeline space and to provide a trajectory planning and tracking method for a wall-climbing robot in pipeline operation and maintenance.

[0009] The purpose of the present invention can be achieved by the following technical solutions:

[0010] A trajectory planning and tracking method for a wall-climbing robot in pipeline operation and maintenance comprises the following steps:

[0011] S1: Establish a cylindrical coordinate system on the pipeline and use it as the world coordinate system; establish a trolley coordinate system on the wall-climbing robot body, calculate the posture state of the wall-climbing robot in the inner wall of the pipeline in real time through sensor data during the operation of the wall-climbing robot, and determine the position relationship of the trolley coordinate system relative to the world coordinate system at any time according to the contact position between the wall-climbing robot and the pipeline wall;

[0012] S2: Establish the kinematic model of the wall-climbing robot on the inner wall of the cylindrical pipe;

[0013] S3: Expand the cylindrical surface coordinate system of the pipeline to obtain a plane coordinate system; determine the current posture of the wall-climbing robot according to the positional relationship between the trolley coordinate system and the world coordinate system, convert the predetermined target posture and the current posture of the wall-climbing robot from the world coordinate system to the plane coordinate system, obtain the shortest path, and convert it to the world coordinate system to obtain the target path;

[0014] S4: Based on the target path obtained in step S3 and in combination with the kinematic model obtained in step S2, a trajectory tracking controller is constructed to enable the wall-climbing robot to track the target trajectory on the pipeline surface.

[0015] Furthermore, the process of establishing the cylindrical coordinate system of the pipeline is specifically as follows: taking a point on the circumference of one end of the pipeline as the origin, the direction of the center of the circle pointing from the point to the circumference as the Y axis, the direction perpendicular to the circumference from the point as the Z axis, and determining the X axis, thereby obtaining the cylindrical coordinate system of the pipeline.

[0016] Furthermore, the process of establishing the trolley coordinate system is specifically as follows: obtain the four contact points between the wall-climbing robot and the pipeline, use the center of the plane formed by the four contact points as the origin of the trolley coordinate system, use the direction perpendicular to the plane toward the top of the trolley as the X-axis direction, and establish mutually perpendicular Y-axis and Z-axis directions according to the plane to obtain the trolley coordinate system.

[0017] Furthermore, the wall-climbing robot comprises a body and four wheels, the four wheels comprising two driving wheels and two driven wheels, and the driving wheels are provided with a motor encoder and a gyroscope;

[0018] The sensor data in step S1 includes gyroscope data and encoder data of the motor encoder. The latest posture state reached by the wall-climbing robot after each control cycle is updated in real time by combining the gyroscope data and encoder data according to the small wheel radius and wheelbase information in each control cycle.

[0019] Furthermore, the positional relationship of the vehicle coordinate system relative to the world coordinate system includes a translation and a rotation relationship, and the rotation relationship is represented by a rotation matrix. The calculation process of the rotation matrix includes:

[0020] Assume that the unit orthogonal basis along each axis in the world coordinate system constitutes a matrix:

[0021] The unit orthogonal basis along each axis in the car coordinate system constitutes the matrix: in,

[0022]

[0023] (A 1f ,A 2f ,A 3f ,A 4f ) are the position coordinates of the contact points between the four wheels of the wall-climbing robot and the pipe surface in the world coordinate system;

[0024] The rotation matrix R from the car coordinate system to the world coordinate system can be calculated as follows:

[0025]

[0026] Furthermore, the process of establishing the kinematic model of the wall-climbing robot on the inner wall of the cylindrical surface of the pipeline includes:

[0027] The kinematic model of the known wheeled differential mobile robot on the curved surface is:

[0028]

[0029] Where (u, v) constitutes a semi-geodesic coordinate grid on the surface, the u direction is the direction of a geodesic line on the surface, and the v direction is orthogonal to the u direction; (u1, ω) represent the linear velocity and angular velocity input respectively; θ represents the angle between the vehicle head direction and the positive direction of u; G is given by the formula Calculated, is the unit vector in the v direction; (·) u It means to find the partial derivative of u;

[0030] A semi-geodesic coordinate grid is established on the cylindrical surface of the pipe. The direction of a geodesic line on the cylindrical surface is obtained along the Z-axis direction of the world coordinate system. The Z-axis corresponds to the u-axis direction of the semi-geodesic coordinate axis. Then, the v-axis orthogonal to the geodesic line can be obtained along the circumferential direction at the origin of the world coordinate system, forming a semi-geodesic coordinate grid on the cylindrical surface of the pipe.

[0031] Cylindrical surface parameters The kinematic model of the wall-climbing robot on the inner wall of the cylindrical pipe is:

[0032]

[0033] Furthermore, in step S3, the target path acquisition process is specifically as follows:

[0034] Establish the corresponding relationship between the pipe cylindrical surface coordinate system and the plane coordinate system: Assume that the plane coordinate system is obtained by cutting the pipe cylindrical surface along the plane (YOZ) and then unfolding it, and establish the coordinate system O on the plane. p (X′Z′), let the origin of the plane coordinate system be at the same position as the origin of the pipe cylindrical surface coordinate system, take the direction of the Z′ axis to be the same as the direction of the Z axis, and the direction of the X′ axis is horizontal to the left;

[0035] By formula The given target posture and the current posture of the wall-climbing robot are converted from the world coordinate system to the plane coordinate system to realize the conversion of the midpoints of the world coordinate system and the plane coordinate system, where R is the radius of the pipeline;

[0036] The parametric equation of the shortest path is calculated in the plane coordinate system and converted to the pipe cylindrical surface coordinate system to obtain a target path.

[0037] Furthermore, the process of establishing the trajectory tracking controller includes:

[0038] According to the kinematic model of the wall-climbing robot on the inner wall of the cylindrical pipe, the actual position of the wall-climbing robot is: g = [u, v, θ] T , then the kinematic model can be transformed into in

[0039] Assume the given reference trajectory is: g r =[u r ,v r ,θ r ] T , the tracking error is expressed as:

[0040]

[0041] Get auxiliary design function Among them, α is the independent variable, k1, k2 are positive constants, α r As a constant, the auxiliary tracking error is expressed as:

[0042]

[0043] Taking the derivative of z with respect to t, we get: in Represents the controller output, which specifically includes the vehicle target linear velocity u1, target angular velocity ω and the independent variable derivative of the auxiliary design function Represents the reference speed of a given trajectory, including the reference linear speed u of the vehicle body 1r and the reference angular velocity ω r ,

[0044] e=[0,0,0] T ;

[0045] according to The expression is used to design the trajectory tracking control law:

[0046]

[0047] where K∈R 3×3 is a positive definite gain matrix, and the obtained control law make Finally, the asymptotic convergence of the trajectory tracking error is achieved.

[0048] Furthermore, the values ​​of the first two dimensions of the trajectory tracking controller output, namely the vehicle target velocity (u1, ω), need to be converted into the given angular velocity values ​​(ω) of the servo motors in the left and right drive wheels. l ,ω r ) and direction vector, the conversion process is as follows:

[0049] Considering that the car moves in the three-dimensional space of the cylindrical surface, the target linear velocity is expressed as a vector form in the three-dimensional space: By formula Converted to linear speed of left and right wheels Where b is the wheelbase;

[0050] By formula Converted to the angular velocity of the left and right wheels (ω l ,ω r ), where r is the wheel radius, is the direction vector of the tangent line at the contact point between the left and right wheels and the cylindrical surface of the pipe;

[0051] The equations of the cylindrical surface of the pipe, the plane equations of the left / right wheels obtained from the spatial coordinate positions of the front and rear wheels, and the spatial circular curve equations of the left / right front wheels are combined to obtain a system of equations. The spatial coordinates of the actual contact points between the left and right wheels and the cylindrical surface of the pipe are obtained by solving them, which are expressed as A 2f ′ and A 1f ′;

[0052] Assume that the center of the left and right wheels in the pipe cylindrical coordinate system is represented by A 2Of and A 1Of , calculate the vector A 2f ′-A 2Of and vector A 2f -A 2Of The angle α between them, vector A 1f ′-A 1Of and A 1f -A 1Of The angle β between them;

[0053] The direction vector of the tangent line at the contact point between the left and right wheels and the cylindrical surface of the pipe is in the trolley coordinate system O m xO of (x,y,z) m The z-plane forms angles α and β with the z-axis respectively, and the direction vector In the car coordinate system, it is expressed as:

[0054] The direction vector can be obtained by the rotation matrix R from the car coordinate system to the world coordinate system: The expression in the world coordinate system is:

[0055] Furthermore, the trajectory planning and tracking method plans a shortest path from the current position of the wall-climbing robot to the target point in a known pipeline environment, and makes the tracking of the reference trajectory by the wall-climbing robot converge asymptotically.

[0056] Compared with the prior art, the present invention has the following advantages:

[0057] (1) The present invention uses the kinematic model of the wheeled differential robot on the curved surface to obtain the kinematic model of the wall-climbing robot on the cylindrical surface of the pipeline, and combines the geometric characteristics of the actual contact between the wheels and the pipeline wall to improve the accuracy of the motion control of the wall-climbing robot in the pipeline;

[0058] (2) Based on the simple geometric correspondence between the cylindrical surface and the plane, the present invention realizes the motion trajectory planning of the shortest distance between two points in the pipeline space;

[0059] (3) The present invention proposes a trajectory tracking controller for a wall-climbing robot that operates autonomously on the cylindrical surface of a pipeline. Compared with the existing system, it can reduce the labor intensity of the staff and has certain significance for realizing the fully autonomous operation mode of the pipeline inspection wall-climbing robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 A schematic flow chart of a trajectory planning and tracking method for a wall-climbing robot in pipeline operation and maintenance provided in an embodiment of the present invention;

[0061] Figure 2 A schematic diagram of the structure of a wall-climbing robot provided in an embodiment of the present invention;

[0062] Figure 3 A schematic diagram of a pipeline provided in an embodiment of the present invention;

[0063] Figure 4 This is a schematic diagram of the running state of a wall-climbing robot in a pipeline provided in an embodiment of the present invention;

[0064] Figure 5A schematic diagram of a u-direction tracking curve provided in an embodiment of the present invention;

[0065] Figure 6 A schematic diagram of a v-direction tracking curve provided in an embodiment of the present invention;

[0066] Figure 7 A schematic diagram of a corner tracking curve provided in an embodiment of the present invention;

[0067] Figure 8 A schematic diagram of a position tracking error curve provided in an embodiment of the present invention;

[0068] Fig. 9 A schematic diagram of a speed tracking error curve provided in an embodiment of the present invention;

[0069] Fig.10 A schematic diagram of pipeline surface trajectory tracking provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0070] In order to make the purpose, 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 in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings here can be arranged and designed in various different configurations.

[0071] Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention claimed for protection, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0072] It should be noted that similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, further definition and explanation thereof is not required in subsequent drawings.

[0073] Example 1

[0074] This embodiment provides a trajectory planning and tracking method for a wall-climbing robot in pipeline operation and maintenance. The method flow is as follows: Figure 1 shown. Figure 2 A schematic diagram of the structure of a wall-climbing robot provided in an embodiment of the present invention, wherein the wall-climbing robot comprises a body and four wheels, wherein the first two wheels are active wheels, i.e., driving wheels, and the rear two wheels are driven wheels, and a differential drive mode is adopted, and a motor encoder and a gyroscope of a driving mechanism are installed thereon. Figure 3 A schematic diagram of a pipeline provided by an embodiment of the present invention. Figure 4 ,The wall-climbing robot is adsorbed on the cylindrical surface of the inner wall of the pipe when it is running.

[0075] The specific implementation process includes the following steps:

[0076] S1: A coordinate system is established on the cylindrical surface of the pipe as the world coordinate system, and a coordinate system is established on the wall-climbing robot body as the trolley coordinate system. During the operation of the wall-climbing robot, the sensor data in the body is obtained in real time, and the posture state of the wall-climbing robot in the inner wall of the pipe is calculated in real time. At the same time, the contact position between the trolley wheels and the pipe wall is calculated to determine the position relationship of the trolley coordinate system relative to the world coordinate system at any time.

[0077] S2: According to the world coordinate system and the trolley coordinate system established in S1, a kinematic model of the wall-climbing robot on the pipe surface is established. The pipe surface is a cylindrical surface with known parameters. According to the motion model of the left and right wheel differential trolley on the surface and the cylindrical surface parameters, the kinematic model of the wall-climbing robot on the cylindrical surface can be obtained.

[0078] S3: Establish the relationship between the cylindrical coordinate system of the pipeline and the plane coordinate system after the cylindrical surface is unfolded. According to the positional relationship between the trolley coordinate system and the world coordinate system obtained in S1, the current posture of the wall-climbing robot is obtained. The given target posture and the current posture of the wall-climbing robot are converted from the world coordinate system to the plane coordinate system. A shortest path is obtained through a straight line between two points in the plane coordinate system. The path expression is then converted into a representation in the world coordinate system to obtain a target path with the shortest distance between two points in the pipeline space.

[0079] S4: Based on the pipeline space target path generated in S3 and the kinematic model of the wall-climbing robot on the cylindrical surface obtained in S2, a trajectory tracking controller is designed to enable the wall-climbing robot to track the target trajectory on the pipeline surface.

[0080] More specifically, the method for establishing the coordinate system in step S1 is as follows: for the cylindrical surface of the pipe, a certain point on the circumference of one end of the pipe is taken as the origin, the direction from the point to the center of the circumference is taken as the Y axis, the direction perpendicular to the circumference of the point, that is, the direction along the height of the pipe cylinder is taken as the Z axis, and finally the X axis direction is determined by the right-hand rule to obtain the cylindrical surface coordinate system O of the pipe. f (X, Y, Z); For the wall-climbing robot body, first obtain the four contact points between the outer edge of the wheel and the curved surface when the wall-climbing robot is placed at the entrance of the pipeline (A 1m ,A 2m ,A 3m ,A 4m), the contact points represent the positions of the four wheels in the coordinate system of the trolley, where 1, 2, 3, and 4 represent the wheels at the right front, left front, left rear, and right rear positions, respectively. Then, the center of the plane formed by the four contact points is taken as the origin of the coordinate system of the trolley, the direction perpendicular to the plane upward is taken as the x-axis direction, the direction from wheel 2 to wheel 1 is taken as the y-axis direction, and the direction from wheel 3 to wheel 2 is taken as the z-axis direction. The coordinate system O of the trolley is obtained. m (x,y,z).

[0081] In step S1, the sensor data in the vehicle body including the gyroscope data and the encoder data of the left and right wheel drive motors are obtained in real time. The latest posture state reached by the wall-climbing robot after each control cycle is updated in real time according to the small wheel radius and wheelbase information in each control cycle in combination with the gyroscope data and the encoder data of the left and right wheel drive motors.

[0082] More specifically, in step S1, the position relationship of the vehicle coordinate system relative to the world coordinate system includes a translation and a rotation relationship, wherein the rotation relationship from the vehicle coordinate system to the world coordinate system is represented by a rotation matrix, and the calculation method is:

[0083] S11: Assume that the unit orthogonal basis along the XYZ axis in the world coordinate system constitutes a matrix:

[0084] S12: The unit orthogonal basis along the xyz axis in the car coordinate system constitutes the matrix: in:

[0085] (A 1f ,A 2f ,A 3f ,A 4f ) is the position coordinate of the contact point between the wheel and the pipe surface in the world coordinate system;

[0086] S13: The rotation matrix R from the car coordinate system to the world coordinate system can be calculated as follows:

[0087] More specifically, the kinematic model of the wall-climbing robot on the cylindrical surface in step S2 is established as follows:

[0088] S21: The kinematic model of the known wheeled differential mobile robot on a curved surface is:

[0089]

[0090] Where (u, v) constitutes a semi-geodesic coordinate grid on the surface, the u direction is the direction of a geodesic line on the surface, and the v direction is orthogonal to the u direction; (u1, ω) represent the linear velocity and angular velocity input respectively; θ represents the angle between the vehicle head direction and the positive direction of u; G is given by the formula Calculated, is the unit vector in the v direction; (·) u It means partial derivative with respect to u.

[0091] S22: A semi-geodesic coordinate grid is established on the cylindrical surface of the pipeline. The direction of a geodesic line on the cylindrical surface can be obtained along the Z-axis direction of the world coordinate system. The Z-axis corresponds to the u-axis direction of the semi-geodesic coordinate axis. Then, the v-axis orthogonal to the geodesic line can be obtained along the circumferential direction at the origin of the world coordinate system, forming a semi-geodesic coordinate grid on the cylindrical surface of the pipeline.

[0092] S23: Parameters of cylindrical surface The kinematic model of the wall-climbing robot on the inner wall of the cylindrical pipe is:

[0093]

[0094] More specifically, the path planning method for the shortest distance between two points in the pipeline space in step S3 is:

[0095] S31: Establish the relationship between the pipe cylindrical surface coordinate system and the plane coordinate system after the cylindrical surface is unfolded: Assume that the plane is obtained by cutting the pipe cylindrical surface along the plane (YOZ) and then unfolding it, and establish the coordinate system O on the plane p (X′Z′), let the origin of the plane coordinate system be at the same position as the origin of the pipe cylindrical surface coordinate system, take the direction of the Z′ axis to be the same as the direction of the Z axis, and the direction of the X′ axis is horizontal to the left;

[0096] S32: By formula The given target posture and the current posture of the wall-climbing robot are converted from the world coordinate system to the plane coordinate system to realize the conversion of the midpoints of the world coordinate system and the plane coordinate system, where R is the radius of the pipeline;

[0097] S33: Calculate the parametric equation of the straight line between the two points in the plane coordinate system, convert it to the pipe cylindrical surface coordinate system through the formula in S32, and obtain a target path.

[0098] More specifically, the design method of the trajectory tracking controller of the wall-climbing robot running on the pipeline surface in step S4 is:

[0099] S41: According to the kinematic model of the wall-climbing robot on the inner wall of the cylindrical pipe, the actual position of the wall-climbing robot is: g = [u, v, θ] T , then the kinematic model can be transformed into in

[0100] S42: Assume the given reference trajectory is: g r =[ur ,v r ,θ r ] T , the tracking error can be expressed as:

[0101]

[0102] S43: Get auxiliary design function , where α is the independent variable, k1, k2 are positive constants, α r As a constant, the auxiliary tracking error is expressed as:

[0103]

[0104] Taking the derivative of z with respect to t, we get: in Represents the controller output, which specifically includes the vehicle target linear velocity u1, target angular velocity ω and the independent variable derivative of the auxiliary design function Represents the reference speed of a given trajectory, including the reference linear speed u of the vehicle body 1r and the reference angular velocity ω r ,

[0105] e=[0,0,0] T ;

[0106] S44: According to The expression can be used to design the trajectory tracking control law:

[0107]

[0108] where K∈R 3×3 is a positive definite gain matrix, and the obtained control law make Finally, the asymptotic convergence of the trajectory tracking error is achieved.

[0109] More specifically, the controller output obtained in step S44 is Contains the target velocity of the vehicle (u1, ω) and the derivative of the independent variable of the auxiliary design function f(α) The vehicle target speed is converted into the given angular velocity value of the left and right driving wheel servo motor (ω l ,ω r ) is:

[0110] S51: Considering that the car moves in the three-dimensional space of the cylindrical surface, the target linear velocity is expressed as a vector form in the three-dimensional space By formula Converted to linear speed of left and right wheels Where b is the wheelbase;

[0111] S52: By formula Converted to the angular velocity of the left and right wheels (ω l ,ω r ), where r is the wheel radius, is the direction vector of the tangent line at the contact point between the left and right wheels and the cylindrical surface of the pipe;

[0112] S53: Combine the equation of the pipe cylinder, the left / right wheel plane equation obtained from the spatial coordinate positions of the front and rear wheels, and the spatial circular curve equation of the left / right front wheels to obtain a system of equations. Solve to obtain the spatial coordinates of the actual contact points between the left and right wheels and the pipe cylinder, expressed as A 2f ′ and A 1f ′;

[0113] S54: Let the center of the left and right wheels be represented by A in the pipe cylindrical coordinate system. 2Of and A 1Of , we can calculate the vector A 2f ′-A 2Of and vector A 2f -A 2Of The angle α between them, vector A 1f ′-A 1Of and A 1f -A 1Of The angle β between them;

[0114] S55: The direction vector of the tangent line at the contact point between the left and right wheels and the cylindrical surface of the pipe is in the trolley coordinate system O m xO of (x,y,z) m The z-plane forms angles α and β with the z-axis respectively, and the direction vector can be obtained In the car coordinate system, it is expressed as:

[0115] S56: The direction vector can be obtained by the rotation matrix R from the car coordinate system to the world coordinate system The expression in the world coordinate system is:

[0116] Here, the obtained kinematic model of the cylindrical surface of the pipeline of the wall-climbing robot is used to set a set of starting points and end points on the pipeline to perform simulation tests on the pipeline surface path planning and trajectory following of the wall-climbing robot to illustrate the effectiveness of the present invention:

[0117] 1) Set the pipeline as a vertical cylindrical surface with a radius of 2m and a height of 10m;

[0118] 2) In the pipe cylindrical surface coordinate system, set the path starting point coordinates to: A = [0; 4; 0], and the end point coordinates to

[0119] A = [0; 0; 10];

[0120] 3) Set the wall-climbing robot to the starting coordinate of the path, with the front of the robot facing vertically upward;

[0121] In this embodiment, the specific parameters of the trajectory tracking control method provided by the present invention are set as shown in Table 1.

[0122] Table 1

[0123]

[0124] Figure 5-Figure 10 The position tracking curve, position tracking error curve, speed tracking error curve and pipeline surface trajectory tracking effect diagram of the wall-climbing robot implementing the trajectory planning and tracking method in the pipeline space under the setting parameters of this embodiment are respectively given. It can be clearly seen from the experimental results that the trajectory planning and tracking method of the wall-climbing robot in the pipeline space provided by the present invention can well realize the path planning in the pipeline space and realize the asymptotic tracking of the reference trajectory by the wall-climbing robot.

[0125] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A trajectory planning and tracking method for a wall-climbing robot in pipeline operation and maintenance, characterized in that: The following steps are involved: S1: Establish a cylindrical coordinate system on the pipeline and use it as the world coordinate system; establish a trolley coordinate system on the wall-climbing robot body, calculate the posture state of the wall-climbing robot in the inner wall of the pipeline in real time through sensor data during the operation of the wall-climbing robot, and determine the position relationship of the trolley coordinate system relative to the world coordinate system at any time according to the contact position between the wall-climbing robot and the pipeline wall; S2: Establish the kinematic model of the wall-climbing robot on the inner wall of the cylindrical pipe; S3: Expand the cylindrical surface coordinate system of the pipeline to obtain a plane coordinate system; determine the current posture of the wall-climbing robot according to the positional relationship between the trolley coordinate system and the world coordinate system, convert the predetermined target posture and the current posture of the wall-climbing robot from the world coordinate system to the plane coordinate system, obtain the shortest path, and convert it to the world coordinate system to obtain the target path; S4: According to the target path obtained in step S3 and in combination with the kinematic model obtained in step S2, a trajectory tracking controller is constructed to enable the wall-climbing robot to track the target trajectory on the pipeline surface; The establishment process of the trajectory tracking controller includes: According to the kinematic model of the wall-climbing robot on the inner wall of the cylindrical pipe, the actual position of the wall-climbing robot is: g = [u, v, θ] T , then the kinematic model can be transformed into in Assume the given reference trajectory is: g r =[u r ,v r ,θ r ] T , the tracking error is expressed as: Get auxiliary design function Among them, α is the independent variable, k1, k2 are positive constants, α r As a constant, the auxiliary tracking error is expressed as: Taking the derivative of z with respect to t, we get: in Represents the controller output, which specifically includes the vehicle target linear velocity u1, target angular velocity ω and the independent variable derivative of the auxiliary design function Represents the reference speed of a given trajectory, including the reference linear speed u of the vehicle body 1r and the reference angular velocity ω r , e＝[0,0,0] T ； according to The expression is used to design the trajectory tracking control law: where K∈R 3×3 is a positive definite gain matrix, and the obtained control law make Finally, the asymptotic convergence of the trajectory tracking error is achieved.

2. The trajectory planning and tracking method of a wall-climbing robot in pipeline operation and maintenance according to claim 1 is characterized in that: The process of establishing the pipeline cylindrical surface coordinate system is specifically as follows: taking a point on the circumference of one end of the pipeline as the origin, the direction from the point to the center of the circle as the Y axis, the direction perpendicular to the circle as the Z axis, and determining the X axis, thereby obtaining the pipeline cylindrical surface coordinate system.

3. The trajectory planning and tracking method of a wall-climbing robot in pipeline operation and maintenance according to claim 1 is characterized in that: The process of establishing the trolley coordinate system is specifically as follows: obtain the four contact points between the wall-climbing robot and the pipeline, use the center of the plane formed by the four contact points as the origin of the trolley coordinate system, use the direction perpendicular to the plane toward the top of the trolley as the X-axis direction, and establish mutually perpendicular Y-axis and Z-axis directions according to the plane to obtain the trolley coordinate system.

4. The trajectory planning and tracking method of a wall-climbing robot in pipeline operation and maintenance according to claim 1 is characterized in that: The wall-climbing robot comprises a body and four wheels, wherein 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 step S1 includes gyroscope data and encoder data of the motor encoder. The latest posture state reached by the wall-climbing robot after each control cycle is updated in real time by combining the gyroscope data and encoder data according to the small wheel radius and wheelbase information in each control cycle.

5. The trajectory planning and tracking method of a wall-climbing robot in pipeline operation and maintenance according to claim 1 is characterized in that: The positional relationship of the vehicle coordinate system relative to the world coordinate system includes translation and rotation relationships. The rotation relationship is represented by a rotation matrix. The calculation process of the rotation matrix includes: Assume that the unit orthogonal basis along each axis in the world coordinate system constitutes a matrix: The unit orthogonal basis along each axis in the car coordinate system constitutes the matrix: in, (A 1f ,A 2f ,A 3f ,A 4f ) are the position coordinates of the contact points between the four wheels of the wall-climbing robot and the pipe surface in the world coordinate system; The rotation matrix R from the car coordinate system to the world coordinate system can be calculated as follows:

6. The trajectory planning and tracking method of a wall-climbing robot in pipeline operation and maintenance according to claim 1 is characterized in that: The process of establishing the kinematic model of the wall-climbing robot on the inner wall of the cylindrical surface of the pipeline includes: The kinematic model of the known wheeled differential mobile robot on the curved surface is: Where (u, v) constitutes a semi-geodesic coordinate grid on the surface, the u direction is the direction of a geodesic line on the surface, and the v direction is orthogonal to the u direction; (u1, ω) represent the linear velocity and angular velocity input respectively; θ represents the angle between the vehicle head direction and the positive direction of u; G is given by the formula Calculated, is the unit vector in the v direction; (·) u It means to find the partial derivative of u; A semi-geodesic coordinate grid is established on the cylindrical surface of the pipe. The direction of a geodesic line on the cylindrical surface is obtained along the Z-axis direction of the world coordinate system. The Z-axis corresponds to the u-axis direction of the semi-geodesic coordinate axis. Then, the v-axis orthogonal to the geodesic line can be obtained along the circumferential direction at the origin of the world coordinate system, forming a semi-geodesic coordinate grid on the cylindrical surface of the pipe. The parameter G in the cylindrical surface is 1, The kinematic model of the wall-climbing robot on the inner wall of the cylindrical pipe is:

7. The trajectory planning and tracking method of a wall-climbing robot in pipeline operation and maintenance according to claim 1 is characterized in that: In step S3, the target path acquisition process is specifically as follows: Establish the corresponding relationship between the pipe cylindrical surface coordinate system and the plane coordinate system: Assume that the plane coordinate system is obtained by cutting the pipe cylindrical surface along the plane (YOZ) and then unfolding it, and establish the coordinate system O on the plane. p (X′Z′), let the origin of the plane coordinate system be at the same position as the origin of the pipe cylindrical surface coordinate system, take the direction of the Z′ axis to be the same as the direction of the Z axis, and the direction of the X′ axis is horizontal to the left; By formula The given target posture and the current posture of the wall-climbing robot are converted from the world coordinate system to the plane coordinate system to realize the conversion of the midpoints of the world coordinate system and the plane coordinate system, where R is the radius of the pipeline; The parametric equation of the shortest path is calculated in the plane coordinate system and converted to the pipe cylindrical surface coordinate system to obtain a target path.

8. The trajectory planning and tracking method of a wall-climbing robot in pipeline operation and maintenance according to claim 1 is characterized in that: The values ​​of the first two dimensions of the trajectory tracking controller output, namely the vehicle target velocity (u1, ω), need to be converted into the given angular velocity values ​​(ω) of the servo motors in the left and right drive wheels. l ,ω r ) and direction vector, the conversion process is as follows: Considering that the car moves in the three-dimensional space of the cylindrical surface, the target linear velocity is expressed as a vector form in the three-dimensional space: By formula Converted to linear speed of left and right wheels Where b is the wheelbase; By formula Converted to the angular velocity of the left and right wheels (ω l ,ω r ), where r is the wheel radius, is the direction vector of the tangent line at the contact point between the left and right wheels and the cylindrical surface of the pipe; The equations of the cylindrical surface of the pipe, the plane equations of the left / right wheels obtained from the spatial coordinate positions of the front and rear wheels, and the spatial circular curve equations of the left / right front wheels are combined to obtain a system of equations. The spatial coordinates of the actual contact points between the left and right wheels and the cylindrical surface of the pipe are obtained by solving them, which are expressed as A 2f ′ and A 1f ′; Assume that the center of the left and right wheels in the pipe cylindrical coordinate system is represented by A 2Of and A 1Of , calculate the vector A 2f ′-A 2Of and vector A 2f -A 2Of The angle α between them, vector A 1f ′-A 1Of and A 1f -A 1Of The angle β between them; The direction vector of the tangent line at the contact point between the left and right wheels and the cylindrical surface of the pipe is in the trolley coordinate system O m xO of (x,y,z) m The z-plane forms angles α and β with the z-axis respectively, and the direction vector In the car coordinate system, it is expressed as: The direction vector can be obtained by the rotation matrix R from the car coordinate system to the world coordinate system: The expression in the world coordinate system is:

9. The trajectory planning and tracking method of a wall-climbing robot in pipeline operation and maintenance according to claim 1, characterized in that: The trajectory planning and tracking method plans a shortest path from the current position of the wall-climbing robot to a target point in a known pipeline environment, and makes the tracking of the reference trajectory by the wall-climbing robot converge asymptotically.

Images