Pipeline robot positioning method based on adaptive extended Kalman filtering

Abstract

The invention relates to the technical field of pipeline robot positioning technologies, in particular to a pipeline robot positioning method based on adaptive extended Kalman filtering. Comprising the steps that S1, the pipeline robot carries out state initialization, state error covariance matrix initialization, measurement noise covariance matrix initialization and process noise covariance matrix initialization; s2, acquiring an observation value of the pipeline robot, predicting and correcting the pose state of the pipeline robot based on Kalman filtering, after the correction process, introducing a posteriori estimation residual matrix to adaptively adjust the value of a measurement noise covariance matrix, and enabling the adjusted measurement noise covariance matrix to enter next iteration; s3, the pose state of the pipeline robot is subjected to continuous iteration prediction and correction, pose data of the pipeline robot are obtained, the positioning error of the pipeline robot is reduced, and the positioning precision is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of pipeline robot positioning technology, and particularly to a pipeline robot positioning method based on adaptive extended Kalman filtering. Background Art

[0002] In recent decades, a large number of old and low-quality pipeline facilities have made the underground pipeline transportation capacity insufficient. The urbanization process requires the development of repair technologies with a wide range of applications, low construction costs, and little environmental impact to solve the pain points of repeated urban excavation.

[0003] Although many studies have been conducted on the positioning algorithms of robots, so far, relatively few studies have been applied to the relative positioning of robots in underground pipelines. The problems faced by the positioning methods in this scenario are as follows: First, the shielding of external signals. The robot works in an underground pipeline and has a strong shielding characteristic for external signals such as GPS, and it is impossible to use the satellite positioning system to obtain the position of the robot; Second, the algorithm using internal sensing sensors for robot positioning is easily affected by factors other than measurement noise; Third, the internal space of the pipeline is narrow and the feature points are sparse, resulting in limited application of positioning methods based on lidar and vision. In summary, it is difficult for existing pipeline robots to accurately position, and the positioning error is large. Summary of the Invention

[0004] In view of the above-mentioned disadvantages of the prior art, the purpose of the present invention is to provide a pipeline robot positioning method based on adaptive extended Kalman filtering to solve the technical problem of large positioning error of pipeline robots.

[0005] To achieve the above purpose, the present invention provides a pipeline robot positioning method based on adaptive extended Kalman filtering, including:

[0006] Step S1: The pipeline robot performs state initialization, state error covariance matrix initialization, measurement noise covariance matrix initialization, and process noise covariance matrix initialization;

[0007] Step S2: Obtain the observation value of the pipeline robot, predict and correct the pose state of the pipeline robot based on Kalman filtering. After the correction process, introduce the posterior estimation residual matrix to adaptively adjust the value of the measurement noise covariance matrix, and the adjusted measurement noise covariance matrix enters the next iteration;

[0008] Step S3: Continuously iterate and predict and correct the pose state of the pipeline robot to obtain the pose data of the pipeline robot.

[0009] Further, the expression of the posterior estimation residual matrix ε k is:

[0010]

[0011] where \(y\) k is the actual observed value, is the posterior estimate of the pose state, and \(g\) k (…) is the linear mapping relationship function between the state space and the observation space.

[0012] Furthermore, during the process of adaptively adjusting the value of the measurement noise covariance matrix \(R\) k , a forgetting factor \(\alpha\) is introduced, and the expression is:

[0013]

[0014] where \(R\) k-1 is the measurement noise covariance matrix at the previous moment, \(\alpha\) is the forgetting factor, and \(\varepsilon\) k is the posterior estimate residual matrix, is the transpose matrix of \(\varepsilon\) k , \(\mathbf{P}\) is the prior error covariance matrix, \(G\) k is the observation matrix, is the transpose matrix of \(G\) k .

[0015] Furthermore, the value range of the forgetting factor is \(0.1\leqslant\alpha\leqslant0.4\) or \(0.6\leqslant\alpha\leqslant0.9\).

[0016] Furthermore, the value of the forgetting factor \(\alpha\) is \(0.8\).

[0017] Furthermore, the initial setting of the process noise covariance matrix \(Q\) k is completed through orthogonal experiment and range analysis method.

[0018] Furthermore, the expression of the process noise covariance matrix \(Q\) k is:

[0019]

[0020] where \(\sum_{xx}\) is the radial velocity, \(\sum_{zz}\) is the yaw angle, \(\sum_{z'z'}\) is the yaw angular velocity, and \(\sum_{zz}\), \(\sum_{xx}\), \(\sum_{z'z'}\) are located in the 6th column, 7th column, and 12th column of the process noise covariance matrix \(Q\) k respectively.

[0021] Furthermore, \(\sum_{xx} = 0.07\), \(\sum_{zz} = 0.05\), \(\sum_{z'z'} = 0.07\).

[0022] The beneficial effects of the present invention are as follows: The posterior estimation residual matrix and forgetting factor introduced in the extended Kalman filtering process update the measurement noise covariance matrix during each iteration, causing the change of the Kalman gain to be affected by this matrix. The introduced forgetting factor α determines whether the update of R k is more biased towards historical data, reducing the positioning error of the pipeline robot and improving the positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 is a schematic structural diagram of the test platform for the pipeline robot positioning method in this example;

[0024] Figure 2 is the system framework of the pipeline robot positioning method in this example;

[0025] Figure 3 is the kinematic model of the pipeline robot test platform in this example;

[0026] Figure 4 is the flow chart of the pipeline robot positioning algorithm in this example. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0027] The following describes the embodiments of the present invention through specific examples. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention.

[0028] This method is applied to a pipeline robot. When the pipeline robot is inside the pipeline, this method realizes the accurate positioning of the pipeline robot. First, a test platform (pipeline robot) for this method is established, as Figure 1 shown. The industrial control computer of the test platform has a memory of 16GB, a hard disk capacity of 1T, a system of Ubuntu20.04 version, a chip of N100, is configured with 6 USB ports, two network ports, and one HDMI interface. Its hub motor is equipped with an encoder, and the IMU (inertial measurement unit) is a ten-axis IMU. In the present invention, only six axes are set to be used. Based on the data of the wheel encoder and the inertial measurement unit, a fusion positioning algorithm based on Kalman filtering is carried out to improve the positioning accuracy of the robot in the case of missing global information and deduce a relatively accurate walking trajectory of the robot.

[0029] As Figure 2 and Figure 4 shown, this embodiment provides a pipeline robot positioning method based on adaptive extended Kalman filtering, including:

[0030] Step S1: Initialize the state of the pipeline robot, the state error covariance matrix, the measurement noise covariance matrix, and the process noise covariance matrix;

[0031] Step S2: Obtain the observed values of the pipeline robot, predict and correct the pose state of the pipeline robot based on Kalman filtering. After the correction process, introduce the posterior estimation residual matrix to adaptively adjust the value of the measurement noise covariance matrix, and the adjusted measurement noise covariance matrix enters the next iteration;

[0032] Step S3: Continuously iterate and predict the pose state of the pipeline robot to obtain the pose data of the pipeline robot.

[0033] Before step S1, it is necessary to first establish the dynamic model, kinematic model, state equation, and observation equation of the pipeline robot.

[0034] As Figure 3 shown, assuming that the speeds of the front wheel, middle wheel, and rear wheel in the pipeline robot model are strictly synchronized, establish the arithmetic relationship between the encoder data and the vehicle body speed, that is, the dynamic model of the pipeline robot system, as follows:

[0035]

[0036] In the formula, v x is the radial speed during the movement of the pipeline robot, ω z is the yaw angular velocity during the movement of the pipeline robot, l is the distance between the two driving wheels of the pipeline robot, v l is the speed of the left wheel of the robot, v r is the speed of the right wheel of the robot.

[0037] Furthermore, establish the kinematic model as follows:

[0038]

[0039] In the formula, represents the rate of change of the position and attitude of the robot over time, is the radial speed of the pipeline robot, is the axial speed of the pipeline robot, is the yaw angular velocity during the movement of the pipeline robot, v is the linear speed of the robot, ω is the angular velocity data of the robot, where v and v x represent the same variable, and ω and ω z represent the same variable.

[0040] Regarding the movement of the pipeline robot as two-dimensional movement, before establishing the state equation of the pipeline robot, first establish its state process X:

[0041] X = [x, y, v x , θ, ω z T

[0042] where x represents the radial position of the robot, y represents the axial position of the robot, and v x is the linear velocity in the direction, θ is the yaw angle, and ω z is the yaw angular velocity.

[0043] Then the state equation of the pipeline robot is established as follows:

[0044] X k+1 = f(X k , v k ) + B k w k+1

[0045] where the subscript k + 1 and the subscript k represent the sampling times, X k+1 is the radial position of the robot at time k + 1, f(…) is the state transition function, v k is the input at time k, B k is the control input matrix, and w k+1 is the process noise of the robot at time k + 1.

[0046] Before establishing the observation equation of the pipeline robot, first establish the measurement value Y of its sensor, which is expressed as:

[0047] Y = [v x2 , θ 2 , ω z2 T

[0048] where v x2 is the linear velocity in the direction from the wheel odometer, θ 2 is the yaw angle from the inertial measurement unit, and ω z2 is the yaw angular velocity from the inertial measurement unit.

[0049] Then the observation equation is established:

[0050] Y k+1 = GX k+1 + G k+1 n k+1

[0051] where G K+1 is the observation matrix, and n k+1 represents the observation noise,

[0052] such as Figure 2 and 4 ​​As shown, the data initialization of the pipeline robot includes pose initialization, Kalman filter initialization, and process noise covariance matrix Q k Initialization.

[0053] Before data initialization, data needs to be acquired, including robot angle data and speed data. In this example, for data filtering and time synchronization, the odometer and sensors used have built-in filtering functions. The synchronization process reads the topics of the IMU and wheel odometer sequentially through a loop and publishes the initial values through different ROS topics respectively.

[0054] The pose initialization of the robot is to give a prior estimate Initial value, and the expression is:

[0055]

[0056] In the formula, is the initial value of the prior estimate of the robot pose, E(…) represents the expectation, and x 0 is the initial value of the robot pose. In this example, the value of x 0 is directly initialized and assigned using the data of the first measurement.

[0057] The Kalman filter initialization is to give the state error covariance matrix Initial value, and the expression is:

[0058]

[0059] In the formula, is the initial value of the error covariance matrix.

[0060] After the pose initialization of the pipeline robot, state prediction is carried out, and its process calculation expression is:

[0061]

[0062] In the formula, is the prior error covariance matrix, F k-1 is the state transition matrix, is the transpose matrix of F k-1 , is the posterior error covariance matrix of the previous moment, Q k is the process noise covariance matrix, B k-1 is the control input matrix of the previous moment, is the transpose matrix of B k-1 , is the prior estimate, is the posterior estimate of the previous moment, v k is the input at time k.

[0063] The process noise covariance matrix Q involved in the prediction process k needs to be initialized. For the initial setting of the process noise covariance matrix Q k in this example, it is completed through orthogonal experiments and range analysis. Among them, the orthogonal experiments are shown in Table 1 and Table 2. First, the Taguchi algorithm is used to design the orthogonal experiment table. In order to simplify the process of selecting the initial value of Q k the orthogonal experiment table is designed with three factors and five levels. The three factors are radial velocity, yaw angle, and yaw angular velocity respectively. The initial values of the five levels are equally spaced values from 0.01 to 0.1.

[0064] Table 1 Orthogonal experiment parameter table

[0065]

[0066] Table 2 Orthogonal experiment results

[0067]

[0068] According to the above orthogonal experiment data, range analysis is carried out. In this example, the sum K of the radial and axial positioning errors of the pipeline robot is used as the expected result for analysis. The range analysis results are shown in Table 3.

[0069] Table 3 Range analysis results

[0070]

[0071]

[0072] After the analysis is completed, the best combination is selected as Σxx = 0.07, Σzz = 0.05, Σz′z′ = 0.07, and the setting of the process noise covariance matrix Q k is completed.

[0073] In the existing technology of multi-dimensional systems, if the process noises corresponding to different state variables are independent, then the process noise covariance matrix is a diagonal matrix, where the elements on the diagonal are the variances of the process noises of each state variable. For example, if the state variables of the system are position and velocity, then the process noise covariance matrix Q k may be expressed as:

[0074]

[0075] In the formula, are the variances of the process noises of position, velocity, and acceleration respectively.

[0076] In this example, in Robot_localization (the robot state estimation node), Σzz, Σxx, and Σz′z′ are located in the 6th, 7th, and 12th columns of the process noise covariance matrix Q k respectively.

[0077]

[0078] The orthogonal array design is used to estimate the influence of multiple factors on the target, simplifying the Q k initial value selection process. In addition, this method obtains better initial values while reducing the number of tests, and the parameter debugging efficiency is 5 times that of the original.

[0079] After the pipeline robot state is predicted, the measurement error is calculated. The expression is:

[0080]

[0081] Then the measurement error is corrected. The expression for the correction process is:

[0082]

[0083] In the formula, d k is the measurement error of the prior estimate, y k is the actual observed value, K k is the Kalman gain, is the prior error covariance matrix, G k is the observation matrix, is the transpose matrix of G k is C k is the transformation matrix of the measurement noise, is the transpose matrix of C k is R k is the measurement noise covariance matrix, is the posterior error covariance matrix, is the posterior estimate of the pose state, is the prior estimate of the pose state, g k (…) is the linear mapping relationship function between the state space and the observation space.

[0084] In this example, the value of the measurement noise covariance matrix R k is adjusted through the idea of self - adaptation. Based on the code in the Robot_localization library in the ROS system, the robot position estimation is implemented. In the actual use process, only the parameter value of the measurement covariance matrix R k needs to be adjusted. That is, the present invention introduces a self - adaptive method to design the value of the measurement noise covariance matrix R k .

[0085] In the Kalman filtering process, the measurement noise covariance matrix R is adaptively adjusted by introducing the posterior estimation residual matrix k parameter values of.

[0086] The introduced posterior estimation residual matrix ε k , the expression is:

[0087]

[0088] In the formula, y k is the actual observation value, is the posterior estimation of the pose state, g k (…) is the linear mapping relationship function between the state space and the observation space.

[0089] According to the posterior estimation residual matrix ε k introduced in this example, R k can be preliminarily estimated. To ensure the positive definiteness of R k , the expression of R k is:

[0090]

[0091] In the formula, is the prior error covariance matrix, G k is the observation matrix, is the transpose matrix of G k , is the transpose matrix of ε k .

[0092] In order to obtain a better positioning result and greatly reduce the workload of experimental parameter debugging, in this example, during the adaptive adjustment of the value of the measurement noise covariance matrix R k , the forgetting factor α is introduced to adaptively estimate the measurement noise covariance matrix R k , and the expression is:

[0093]

[0094] In the formula, R k-1 is the measurement noise covariance matrix at the previous moment, α is the forgetting factor, ε k is the posterior estimation residual matrix, is the transpose matrix of ε k , is the prior error covariance matrix, G k is the observation matrix, is the transpose matrix of G k .

[0095] In this example, the posterior estimation residual matrix ε is introduced in the extended Kalman filtering processk and forgetting factor α, the measurement noise covariance matrix is updated during each iteration, such that the change in the Kalman gain is affected by this matrix, where the introduced forgetting factor α determines whether the update of R k is more biased towards historical data.

[0096] A comparative experiment was conducted between a pipeline robot using the existing technology Extended Kalman Filter (EKF) and a pipeline robot using the proposed method Adaptive Extended Kalman Filter (AEKF). The pipeline robot platforms of both walked around a loop corridor with a total distance of no less than 120 meters. The positioning error when returning to the origin again was recorded. Three experiments were conducted each time the parameters were changed, and the average error was calculated as the final result.

[0097] Table 4 Comparison of positioning results

[0098]

[0099] As shown in Table 4, the positioning error of the pipeline robot using the proposed method is much smaller than that of the pipeline robot using the existing technology. In addition, from the positioning results of the final Adaptive Extended Kalman Filter (AEKF) algorithm, it can be seen that different forgetting factors have a greater impact on the positioning results. When the value range of the forgetting factor is 0.1 ≤ α ≤ 0.4 or 0.6 ≤ α ≤ 0.9, the positioning error is significantly reduced compared to the positioning error of the existing technology. When α = 0.8, the positioning error is 0.35 m. At a walking distance of 120 meters, the error is only 0.29%. Compared with the Extended Kalman Filter (EKF) algorithm, the positioning accuracy is improved by approximately 3.26 times.

[0100] The above embodiments merely illustrate the principles and effects of the present invention and are not intended to limit the present invention. Any person familiar with this technology can modify or change the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or changes completed by those with ordinary knowledge in the technical field without departing from the spirit and technical ideas disclosed by the present invention should still be covered by the claims of the present invention.

Claims

1. A pipeline robot positioning method based on adaptive extended Kalman filtering, characterized in that: include: Step S1: The pipeline robot performs posture state initialization, state error covariance matrix initialization, measurement noise covariance matrix initialization and process noise covariance matrix initialization; Step S2: Obtain the observation value of the pipeline robot, predict and correct the position and posture state of the pipeline robot based on Kalman filtering, and after the correction process, introduce the posterior estimation residual matrix to adaptively adjust the value of the measurement noise covariance matrix, and the adjusted measurement noise covariance matrix enters the next iteration; Step S3: Continuously iteratively predict and correct the position and posture state of the pipeline robot to obtain the position and posture data of the pipeline robot.

2. The method according to claim 1, characterized in that The posterior estimated residual matrix ε k The expression is: In the formula, y k is the actual observed value, is the posterior estimate of the pose state, g k (…) is the linear mapping function between state space and observation space.

3. The method according to claim 2, characterized in that In the measurement noise covariance matrix R k In the process of adaptively adjusting the value of , the forgetting factor α is introduced, and the expression is: In the formula, R k-1 is the measurement noise covariance matrix of the previous moment, α is the forgetting factor, ε k is the posterior estimation residual matrix, is k The transposed matrix of is the prior error covariance matrix, G k is the observation matrix, G k The transposed matrix of .

4. The method according to claim 3, characterized in that The range of the forgetting factor is 0.1≤α≤0.4 or 0.6≤α≤0.

9.

5. The method according to claim 4, characterized in that The value of the forgetting factor α is 0.

8.

6. The method according to claim 1, characterized in that Process noise covariance matrix Q k The initial setting is completed through orthogonal experiment and range analysis.

7. The method according to claim 6, characterized in that Process noise covariance matrix Q k The expression is: Where Σxx is the radial velocity, Σzz is the yaw angle, Σz′z′ is the yaw angular velocity, and Σzz, Σxx, and Σz′z′ are located in the process noise covariance matrix Q k The 6th, 7th and 12th columns.

8. The method according to claim 7, characterized in that Σxx=0.07, Σzz=0.05, Σz′z′=0.07.

Images