Body arc length change speed determination method and motion state estimation method for robot climbing pipeline

Abstract

The invention discloses a robot climbing pipeline body arc length change speed determination method and a motion state estimation method, and belongs to the technical field of robot motion control. In order to solve the problem that the motion speed of a robot cannot be effectively estimated because the head of the robot is in a rapid rotation state when the robot crawl a pipeline, effective data are screened out based on time and space sequence data of sensor pressure pulses, an effective data set is projected on an arc length and time plane (s, t), and a spectral clustering algorithm is adopted to calculate the motion speed of the robot. Dividing data points into q geometric clusters by constructing Laplacian matrix characteristic decomposition; performing linear fitting on a cluster with the most data points in the clustering result to obtain a slope ks, namely the body arc length change speed; and the moving speed of the robot along the pipeline axis, the rotating speed around the pipeline axis and the slip rate are estimated based on ks.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of robot motion control, and particularly relates to a robot motion state estimation method with tactile perception. BACKGROUND

[0002] Wheelless robots have adaptability in complex environments. In the condition of having tactile feedback, the robot can perceive pressure changes through sensors to obtain environmental information for autonomous obstacle avoidance. However, in some complex environments, it is difficult for the pressure sensor to accurately obtain obstacle information.

[0003] Due to the light weight and small volume of the thin film pressure sensor, it is usually used for pressure detection on a mobile robot. However, for a wheelless robot which is a multi-joint bionic and super-redundant degree of freedom robot, the current thin film pressure sensor is difficult to perfectly realize the pressure sensing function on its surface.

[0004] In addition, when the robot spirally climbs on a horizontal pipeline, the sensors such as laser radar and camera installed on the head are difficult to position themselves due to the fast rotation of the head, so that the motion speed of the robot cannot be effectively estimated. SUMMARY

[0005] The application aims to solve the problem that the motion speed of the robot cannot be effectively estimated when the robot climbs a pipeline with the head in a fast rotating state.

[0006] A method for determining the change speed of the body arc length of a robot climbing a pipeline, the robot body comprising a plurality of joints, a pressure sensor being arranged on the circumferential side wall of each joint; the method comprising:

[0007] (a) obtaining time and space sequence data of the sensor pressure pulse , and screening out effective data ;

[0008] (b) projecting the effective data set on the arc length and time plane (s, t), the s-axis corresponding to the arc length being the arc length of the pressure sensor relative to the head of the robot; adopting a spectral clustering algorithm, the data points are divided into q geometric clusters by constructing a Laplacian matrix feature decomposition;

[0009] (c) linearly fitting the cluster with the most data points in the clustering result to obtain the slope k s , that is, the change speed of the body arc length of the robot climbing the pipeline.

[0010] Further, the time and space sequence data of the sensor pressure pulse , ​Let be the arc length coordinate of the i-th pressure sensor relative to the robot head; This is the peak timestamp of the m-th pressure pulse detected by the sensor; Let be the peak amplitude of the m-th pressure pulse; This represents the total number of pressure sensors that may come into contact with the pipe when climbing it. This represents the total number of valid pulses detected by sensor i.

[0011] Further, select valid data. During the process greater than the pressure threshold As valid data.

[0012] Furthermore, the process of dividing the data points into q geometric clusters by constructing the Laplacian matrix eigenvalue decomposition using the spectral clustering algorithm includes: based on the dataset {x1, x2, ..., xt} containing M data points (s, t), M} Calculate the similarity matrix Elements in matrix W Point and Similarity; the degree matrix D is defined as a diagonal matrix, with elements on the main diagonal. Let represent the degree of vertex i. Then the normalized random walk Laplace matrix is: , where I is the identity matrix;

[0013] right Perform eigenvalue decomposition and select the eigenvectors v1, v2, ..., v corresponding to the first q smallest eigenvalues. q The eigenvector matrix V = [v1, v2, ..., v] is formed. q Clustering is performed on the row vectors of the eigenvector matrix V to obtain q clusters C={C1,C2,…C…} q}

[0014] Furthermore, a linear fit is performed on the cluster with the most data points in the clustering results to obtain the slope k. s The process includes:

[0015] Select the cluster C with the most data points from all the clusters in the classification. max Weighted least squares is used for C max Perform linear fitting on the data in the dataset;

[0016] Assign weights to each data point based on the corresponding pressure peak:

[0017]

[0018] In the formula, The weight coefficient for the m-th data in the least squares method; For cluster C max The peak pressure at the m-th data point; For cluster C max The number of data points in the data;

[0019] right The optimal slope is obtained by solving the problem. .

[0020] A method for estimating the motion state of a robot climbing a pipe, including estimating the robot's velocity v along the pipe axis. ad,calc Steps;

[0021] The method for determining the rate of change of the robot's body arc length during pipe climbing is used to obtain the rate of change k of the robot's body arc length during pipe climbing. s and then utilize Estimate the movement speed, r s h is the radius of the fitted spiral curve during the robot's climbing process. s The pitch is the screw pitch.

[0022] Furthermore, the method for estimating the motion state of a robot climbing a pipe also includes estimating the robot's rotational speed ω around the pipe axis. ad,calc The steps, in this process, utilize Estimate the rotational speed ω of the robot about the pipe axis. ad,calc .

[0023] Furthermore, the method for estimating the motion state of a robot climbing a pipe also includes estimating the robot's slip ratio. The steps, in this process, utilize Estimating robot slip ratio ,in This represents the theoretical speed at which the robot moves along the pipe axis. .

[0024] Alternatively, the method for estimating the motion state of a robot climbing a pipe may further include estimating the robot's slip ratio. The steps, in this process, utilize Estimating robot slip ratio , where w snake ω is the width of the robot. control This is the robot's gait frequency, which is the reciprocal of the gait cycle.

[0025] Beneficial effects:

[0026] The present application estimates the climbing speed of the robot by using the pressure sensing function of the robot, and can accurately estimate the climbing speed of the robot even when the head is in a fast rotating state, and can also estimate the slip rate of the robot when spirally climbing on a horizontal pipeline. BRIEF DESCRIPTION OF DRAWINGS

[0027] Figure 1 A schematic diagram for the robot climbing on a horizontal pipeline.

[0028] Figure 2 A graph of the peak value data change of the pressure sensor when spirally climbing for 3 cycles.

[0029] Figure 3 A graph of the relationship between the position of the robot body contacting the pipeline and time.

[0030] Figure 4 A graph of weighted linear fitting of the clustered two-dimensional point clusters. DETAILED DESCRIPTION

[0031] In the prior art, when the robot spirally climbs on a horizontal pipeline, the sensors such as laser radar and camera mounted on the head are difficult to perform self-positioning due to the fast rotating state of the head, so the motion speed of the robot cannot be effectively estimated. The present application estimates the climbing speed of the robot by using the pressure sensing function of the robot, and this method can also estimate the slip rate of the robot when spirally climbing on a horizontal pipeline. The present application will be described in detail below in combination with specific embodiments. Specific embodiment one:

[0033] The present embodiment is a method for determining the arc length change speed of the robot body climbing a pipeline, which is also the method for determining the arc length change speed k s of the robot body in specific embodiment two.

[0034] The method for determining the arc length change speed of the robot body climbing a pipeline according to the present embodiment comprises the following steps

[0035] (a) Extract and screen pressure feature data:

[0036] First, the time and spatial sequence data of the sensor pressure pulse are established as:

[0037]

[0038] In the formula, is the arc length coordinate of the i-th pressure sensor relative to the head of the robot; is the peak timestamp of the m-th pressure pulse detected by the sensor; is the peak amplitude of the m-th pressure pulse; The number of pressure sensors that can come into contact with the pipe while climbing the pipe; The total number of valid pulses detected by sensor i.

[0039] Setting a pressure threshold Filtering sensor data located near the top of the pipe as valid data:

[0040]

[0041] (b) Classifying data features using spectral clustering:

[0042] Projecting the valid data set X need on the arc length and time plane (s, t), assuming that the robot can wrap the pipe q times, due to the characteristics of the robot's spiral motion, each data point in the valid data set will be distributed on q straight lines in the (s, t) plane, and the slope of these straight lines represents the rate of change of the robot's body arc length directly above the pipe. Using a spectral clustering algorithm based on graph theory principles, the data points are divided into q geometric clusters by constructing a Laplacian matrix and performing eigenvalue decomposition. Each cluster corresponds to a straight line. Compared with traditional clustering algorithms such as K-nearest neighbors, spectral clustering can handle complex data structures and is suitable for linear cluster separation problems with specific geometric constraints. It also has certain robustness to noise and outliers.

[0043] According to the data set {x1, x2,…, x M} containing M data points (s, t), the similarity matrix is calculated. The elements of matrix W represent the similarity between points , which is usually defined using a Gaussian kernel function:

[0044]

[0045] where is a scale parameter that controls the decay rate of similarity.

[0046] The degree matrix D is defined as a diagonal matrix, and the main diagonal elements represent the degree of vertex i. Then the normalized random walk Laplacian matrix is:

[0047]

[0048] where I is the identity matrix.

[0049] Perform eigenvalue decomposition on and select the eigenvectors v1, v2,…,v q, constitute a feature vector matrix V = [v1, v2, …, v q ], each row corresponds to the coordinates of the original data points in the low-dimensional space, which is actually the first data of v1, v2, …, v q The first row, and so on. Perform K-neighbor clustering on the row vectors of the feature vector matrix V (i.e. the dimensionality reduced data) to finally obtain q clusters C = {C1, C2, … C q} containing all data.

[0050] (c) Least squares linear fitting is performed on the clustering results to obtain k s :

[0051] Select the cluster C max with the most data points from all classified clusters as the reliable data that best reflects the main motion trend for speed estimation, and perform linear fitting on the data in C max using weighted least squares.

[0052] According to the pressure peak corresponding to the data points, the weight of each point is assigned:

[0053]

[0054] In the formula, is the weight coefficient of the mth data in the least squares method; is the pressure peak of the mth data point in the cluster C max ; is the number of data points in the cluster C max .

[0055] The data point distribution should satisfy the linear relationship , and the optimal slope is solved:

[0056]

[0057] Accordingly, the mathematical calculation of the robot body arc length movement speed parameter located directly above the pipeline can be realized, and then used to estimate the actual movement speed and slip rate of the robot along the pipeline axis. Specific implementation method two:

[0059] The motion state estimation method for a robot climbing a pipeline according to the embodiment comprises the following steps:

[0060] (1) Calculate the no-slip theoretical speed:

[0061] As shown in Figure 1 , the helical rotation of the robot is divided into two parts: movement along the pipeline axis and rotation around the pipeline axis; let the movement speed of the robot in the helical direction be vsd The radius of the fitted spiral curve is r. s The pitch is h s Then the robot's speed v along the pipe axis ad for:

[0062]

[0063] The rotational speed ω of the robot about the pipe axis ad for:

[0064]

[0065] For pipe climbing motion with known parameters, a robot with square joints (the joint cross-section is square) moves at a speed v along the helical direction. sd The following formula can be used to calculate it:

[0066]

[0067] In the formula, w snake ω is the width of the robot. control α is the robot's gait frequency, which is the reciprocal of the gait cycle; slip The slip ratio of the motion;

[0068] Without relying on external sensors, the robot itself cannot identify the slip ratio; therefore, the slip ratio α is assumed to be used in theoretical calculations. slip = 0. Therefore, the theoretical speed at which the robot moves along the pipe axis when the slip ratio is 0 is:

[0069]

[0070] It should be noted that: since this embodiment is based on the estimation of a robot with square joints, when using hexagonal prisms, octagonal prisms, or cylindrical joints, the speed can be calculated based on the actual joint shape. ; The number of faces in a joint is set to 6 for a hexagonal prism and 8 for an octagonal prism. For cylindrical joints, the circumferential dimension is determined based on the actual circumferential size and requirements. Similarly, the subsequent process can be based on... To achieve an estimate of the actual speed.

[0071] (2) Estimating the speed of pressure perception:

[0072] When the robot spirally climbs the horizontal pipe, the sensor on the upper side of the pipe will produce a significant pulse signal affected by gravity, while the signal on the lower side is weak. By analyzing the time series and spatial distribution of the pressure pulse peak of the sensor near the upper side of the pipe, the position change of the robot body above the pipe can be detected, and then the moving speed of the robot along the pipe axis and the slip between the robot and the pipe can be calculated.

[0073] Let the change speed of the robot body arc length above the pipe be k s , the calculated rotating speed ω of the robot around the pipe axis ad,calc can be calculated as:

[0074]

[0075] The calculated moving speed v of the robot along the pipe axis ad,calc is calculated by the following formula:

[0076]

[0077] The slip rate can be calculated by the following formula:

[0078]

[0079] According to the change speed k of the robot body arc length s , the slip rate is obtained, so as to determine the estimated motion state of the robot climbing the pipe. It should be noted that: generally, the motion state is estimated to obtain the actual motion speed and the slip rate, because the theoretical motion speed is usually calculated as known (of course, the theoretical motion speed can be considered as a quantity of motion state estimation), so in some embodiments, the actual motion speed is calculated, and then the slip rate can be obtained according to the speed. In some embodiments, the slip rate can also be directly calculated according to the calculated data, etc. according to , in fact, they are essentially the same, only different calculation formulas can be used for calculation.

[0080] Embodiment

[0081] The data change of 32 pressure sensors on the surface of the robot in contact with the pipe when the robot spirally climbs the horizontal pipe is used to estimate the motion speed of the robot. The robot is controlled to spirally climb the horizontal pipe with a diameter of 100 mm at a joint angle frequency of 0.25 rad / s for about 3 motion periods, with a total use time of 75.79 s, the robot actually moves 925 mm along the pipe, the control radius parameter r s = 95 mm, h s= 180mm, using The pressure data collected during the movement process was extracted, and the minimum threshold F th was set to 2.8N, and the effective data set X need was obtained, which represents the change of the sensor pressure peak value near the top of the pipeline, and the data is plotted as shown in Figure 2 .

[0082] Observing Figure 2 , it can be seen that the pressure peak value can be divided into two clusters, and each cluster presents a linear distribution over time, because the robot wraps the pipeline about q = 2 turns, and there are two places on the top of the pipeline where the robot body moves constantly. Each sensor index is converted into the length position information of the robot where the sensor is located, and the effective data set X need is projected on the arc length and time plane (s, t), and the relationship between the sensor position contacted with the pipeline and the time is shown in Figure 3 .

[0083] In Figure 3 , each two-dimensional point represents that the robot body position has a large pressure contact with the pipeline at the current time, and the category in the figure is the classification result calculated by the spectral clustering algorithm described in the foregoing after standardizing the two-dimensional point data. According to the weight formula of the pressure peak value, the data in the category with the largest number is weighted and linearly fitted, and the fitting result is shown in Figure 4 .

[0084] In Figure 4 , the pressure peak value corresponding to each two-dimensional point is taken as its weight for linear fitting, and the slope k s = -0.0038216m / s obtained by fitting in this way represents the moving speed of the robot body located directly above the pipeline, which is substituted into the moving speed v ad,calc formula, and the robot moving speed v ad,calc along the pipeline axis can be calculated as 12.133mm / s. According to the total movement time 75.79s and the total displacement 925mm of the robot, the actual moving speed v ad,real of the robot can be obtained as 12.205mm / s. The moving speed of the robot obtained by the proposed speed estimation algorithm is -0.072mm / s different from the actual moving speed, and the error is -0.59%. It is basically consistent with the true value.

[0085] Further experiments were conducted on the robot speed estimation. The robot was set to have joint angular frequencies of 0.25, 0.375 and 0.5 respectively, and the corresponding gait cycle times were 25.133 s, 16.755 s and 12.566 s respectively. Each group was tested for 3 times, and each time the robot was controlled to move for 3 gait cycles. The robot spiral radius was set to 95 mm, and the pitch was set to 180 mm. According to the data evaluation, the minimum threshold F was set to 2.8 N, which was optimal for the robot prototype speed detection results in this paper. th

[0086] The 9 groups of test experiments were calculated and analyzed, and the final experimental data are shown in Table 1. In the table, the motion cycle refers to the time required for the robot to run one gait; the total displacement refers to the actual distance moved by the robot along the pipeline axis in the total time; the actual speed is the total displacement divided by the total time; the detected speed is the moving speed of the robot obtained by the speed estimation algorithm proposed in the foregoing; the error is the error percentage between the detected speed and the actual speed; the no-slip speed refers to the moving speed that the robot should have if there is no slip between the robot and the pipeline, which is used to calculate the actual slip rate of the robot and the pipeline; the time unit is second, the displacement unit is millisecond, and the speed unit is millimeter per second.

[0087] Table 1: Crawl pipeline speed detection table

[0088]

[0089] In Table 1, compared with the no-slip speed calculated directly according to the robot control parameters, the detection obtained by combining the speed estimation algorithm of tactile perception is closer to the actual value, and in all experiments, the maximum error is 5.17%. Using this speed detection algorithm, the robot can more accurately obtain the speed value of spiral climbing the pipeline in a period of time, and the error of the detection result mainly comes from the accuracy of the pressure sensor and the accuracy of the robot force model, and this error does not increase with time, which can better adapt to the scene of the robot climbing long-distance pipelines. Using this method can also estimate the slip rate between the robot and the pipeline, which is beneficial to more accurate adjustment of the motion control of the robot in the pipeline, thereby improving the work efficiency and accuracy of the robot.

[0090] ​The above calculation examples of the present application are only used to illustrate the calculation model and calculation process of the present application, and are not used to limit the embodiments of the present application. Based on the above description, other different forms of changes or variations can be made by those skilled in the art, and all the embodiments cannot be exhausted here. Any obvious changes or variations derived from the technical solutions of the present application are still within the protection scope of the present application.

Claims

1. A method for determining the velocity of change of the body arc length of a robot climbing a pipe, characterized in that, The robot body comprises a plurality of joints, and a pressure sensor is arranged on the circumferential side wall of each joint The method comprises: (a) acquiring time and space sequence data of sensor pressure pulse and filtering out valid data ; (b) the effective data set The data points are divided into q geometric clusters by constructing a Laplacian matrix and eigen decomposition using a spectral clustering algorithm. (c) linear fitting the cluster with the most data points in the clustering result to obtain a slope k s i.e. the speed of change of the body arc length of the robot climbing the pipeline.

2. The method of claim 1, wherein, Time and spatial sequence data of sensor pressure pulses , is the arc length coordinate of the ith pressure sensor relative to the robot head; is the peak timestamp of the mth pressure pulse detected by the sensor; is the peak amplitude of the mth pressure pulse; is the number of pressure sensors that can potentially come into contact with the pipe while climbing the pipe; is the total number of valid pulses detected by sensor i.

3. The method of claim 2, wherein the method further comprises: determining a change in the arc length of the body of the robot over time; and determining a change in the arc length of the body of the robot over time based on the determined change in the arc length of the body of the robot over time. selecting valid data in the process of greater than a pressure threshold as valid data.

4. The method of claim 2, wherein, The process of dividing data points into q geometric clusters using a spectral clustering algorithm by constructing the Laplacian matrix eigenvalue decomposition includes: Based on a dataset {x1, x2, ..., xt} containing M data points (s, t), ... M } Calculate the similarity matrix Elements in matrix W Point and Similarity; the degree matrix D is defined as a diagonal matrix, with elements on the main diagonal. Let represent the degree of vertex i. Then the normalized random walk Laplace matrix is: , where I is the identity matrix; To perform eigen decomposition, select the eigenvectors v1, v2, …, v q corresponding to the first q smallest eigenvalues to form an eigenvector matrix V = [v1, v2, …, v q ], perform clustering on the row vectors of the eigenvector matrix V, and finally obtain q clusters C = {C1, C2, … C q}.

5. The method according to any one of claims 2 to 4, wherein, Linear fitting of the cluster with the most data points in the clustering result to obtain the slope k s The process includes: selecting a cluster C with the most data points from all the clusters max performing a linear fit on the data in C max using weighted least squares The weight of each point is assigned according to the pressure peak value corresponding to the data point: wherein is the weight coefficient of the mth data in the least square method; is the pressure peak value of the mth data point in the cluster C max is the number of data points in the cluster C max ​​ Solving for yields the optimal slope .

6. A method of estimating a motion state of a robot climbing a pipe, characterized by, comprising the step of estimating the speed of movement v of the robot along the axis of the pipe ad,calc ; The method for determining the body arc length change speed of the robot climbing the pipeline according to any one of claims 1 to 5 s , and further using to estimate the moving speed, r s is the radius of the spiral curve fitted in the robot climbing process, h s is the pitch.

7. The method of claim 6, wherein, Also included is the step of estimating the rotational speed ω of the robot around the pipe axis ad,calc , in the course of which the rotational speed ω of the robot around the pipe axis is estimated ad,calc .

8. The method of claim 6 or 7, wherein Also included is the step of estimating the robot slip rate in which the robot slip rate is estimated in which the robot slip rate is estimated wherein is the theoretical movement speed of the robot along the axis of the pipe.

9. The method of claim 8, wherein, Theoretical movement speed of the robot along the axis of the pipe .

10. The method of claim 6 or 7, wherein Also included is the step of estimating the robot slip rate in which the robot slip rate is estimated where w snake is the width of the robot and ω control is the gait motion frequency of the robot, i.e. the inverse of the gait motion period.

Images