An EKF-based real-time localization method for pipeline inspection robots assisted by lidar

Abstract

The present invention discloses a method for real-time positioning assisted by lidar of a pipeline inspection robot based on EKF, which includes establishing a dynamic model; placing the robot in a ventilation pipeline to collect point cloud data and manually annotating it to train an artificial neural network; initializing the robot state and covariance at the starting working point; predicting the state vector and covariance through the system equation; collecting point cloud data by lidar and performing distortion removal processing; performing inverse calculation to calculate the robot's orientation angle and lateral offset; updating the Kalman gain, robot system state and covariance from the measured values. This method significantly reduces the distortion phenomenon of point clouds due to material reflection. At the same time, the feature extraction method of the present invention significantly reduces the running time and resource consumption, and can be applied to embedded robots; it can achieve good results in the background of complex pipeline environments, which is beneficial to eliminating the cumulative error of self-positioning of pipeline robots in complex pipeline environments and improving the positioning accuracy of pipeline robots.

Description

Technical Field

[0001] The present invention belongs to the technical field of pipeline robot positioning, and particularly relates to a lidar-assisted real-time positioning method for pipeline inspection robots based on EKF. Background Technique

[0002] Ventilation systems with enhanced cleaning and filtration have become an important way to prevent the epidemic. The disinfection and cleaning of ventilation ducts are the key to ensuring air quality. The application of pipeline robots can replace manual labor for disinfection and cleaning operations, which can greatly reduce labor costs and potential biological infection risks. For autonomous pipeline robots, positioning is an essential function. Currently, the existing positioning technologies for pipeline robots are divided into two categories. One category uses magnetic and electric marking points pre-buried in the pipeline for positioning. However, this method requires manual arrangement of signal source points during pipeline laying, which greatly increases the cost. Another category of positioning technologies that do not rely on signal source points usually use a combination of odometers and inertial measurement units for positioning. This type of method has high requirements for sensor performance. The narrow ventilation ducts have great limitations on the selection of sensors. Moreover, due to the complex environment inside the ventilation ducts, it is extremely easy to cause wheel slippage and generate cumulative errors. At the same time, low-cost inertial measurement units have large detection errors for angle measurement. Therefore, there is an urgent need for a positioning method applicable to ventilation duct inspection robots that does not rely on external positioning data sources. Summary of the Invention

[0003] The purpose of the present invention is to solve the problem that the positioning of pipeline robots in complex and airtight ventilation ducts during autonomous operation is prone to cumulative errors, to realize the de-distortion processing and feature extraction of the point cloud reflection on the pipe wall, and to fuse the robot system dynamics model through an extended Kalman filter to perform more accurate real-time positioning of the robot.

[0004] In order to achieve the purpose of the present invention, the present invention discloses a lidar-assisted real-time positioning method for ventilation duct inspection robots based on the extended Kalman filter (EKF), including the following steps:

[0005] Step 1, establish a dynamics model of the ventilation duct inspection robot system;

[0006] Step 2, place the robot in the ventilation duct to collect point cloud data and manually annotate it, and train an artificial neural network of the reflection distortion model of the lidar point cloud with respect to the ventilation duct wall;

[0007] Step 3, initialize the robot state and covariance at the starting working point;

[0008] Step 4, predict the state vector and covariance through the system equation;

[0009] Step 5: Collect point cloud data through the lidar installed on the side of the robot, and perform distortion removal on the distorted point cloud in the ventilation duct environment based on the reflection distortion correction artificial neural network;

[0010] Step 6: Extract line features based on the lidar point cloud intensity and density, and perform inverse solution to calculate the robot's orientation angle and lateral offset;

[0011] Step 7: Update the Kalman gain, robot system state, and covariance from the measurement values.

[0012] Furthermore, for establishing the dynamic model of the ventilation duct inspection robot system in Step 1, the specific steps are as follows:

[0013] Step 1-1: Establish the sensor models of the pipeline robot's gyroscope and encoder:

[0014]

[0015] Among them, ω is the angular velocity of the pipeline robot, ω z is the output angular velocity of the robot's gyroscope, b z is the gyroscope zero bias, v x and v y are the velocities of the robot along the x-axis and y-axis in the navigation coordinate system respectively, and v and θ are the output velocity and heading angle of the encoder of the robot in the navigation coordinate system;

[0016] Step 1-2: Use the sensor models of the robot's gyroscope and encoder to establish the dynamic model of the robot system as:

[0017]

[0018] Among them, the subscript k and the subscript k-1 represent the sampling times, δt represents the sampling time interval, x k 、y k 、v xk 、v yk 、θ k represent the coordinates of the robot along the x-axis direction, the coordinates along the y-axis direction, the velocity along the x-axis direction, the velocity along the y-axis, and the heading angle at the kth moment in the navigation coordinate system respectively.

[0019] Furthermore, for placing the robot in the ventilation duct to collect point cloud data and manually annotating it, and training the artificial neural network of the reflection distortion model of the lidar point cloud for the ventilation duct wall in Step 2, the specific steps are as follows:

[0020] Step 2-1: Place the robot in the working environment, and recombine the distance attribute of the collected point cloud data with the point cloud intensity and angle after manually annotating and modifying it according to the true value of the manual measurement to construct the training set t;

[0021] Step 2-2: Construct a fully connected network M with a multi-layer perceptron structure, specifically as follows:

[0022]

[0023] Among them, vectors x and y are the input layer and output layer of the network respectively. The x vector is composed of the angle and intensity of the point cloud, and the y vector represents the distance attribute of the point cloud. The features of the network are represented by weights υ j 、ω i 、bias b i 、λ and activation functions σ, ρ parameters. Among them, the activation functions are the tansig function and the purelin function respectively;

[0024] Step 2-3: Calculate the error between the output result of network M and the true value according to the loss function J, and iteratively train the network parameters, specifically as follows:

[0025]

[0026] Among them, N is the number of point clouds, y n is the distance of the output point cloud of the network, and t n is the distance value of the point cloud in the data set.

[0027] Furthermore, in step 3, initialize the robot state and covariance at the starting working point. The specific steps are as follows:

[0028] Define the error state vector between the dynamic state and the true state of the robot system as:

[0029]

[0030] Among them, δX is the defined error state vector, and δP, δV, δE are the robot position, velocity, heading angle and sensor measurement errors respectively. Specifically:

[0031]

[0032]

[0033]

[0034] δx and δy are the position coordinate errors of the robot in the navigation coordinate system respectively; δv x 、δv y are the velocity component errors of the robot along the x-axis and y-axis in the navigation coordinate system respectively; δθ, δb od 、δb z are the heading angle error, the encoder cumulative error and the gyroscope drift error respectively; The system covariance is determined by the performance of the encoder and gyroscope assembled on the robot, and T is the matrix transpose symbol.

[0035] Further, predicting the error state vector and covariance through the system equation in step 4 is specifically as follows:

[0036]

[0037] Among them,

[0038]

[0039]

[0040]

[0041]

[0042] In the formula, I 2×2 is a 2-order identity matrix, 0 2×2 , 0 2×3 and 0 3×2 are corresponding zero matrices, δP k-1 , δV k-1 , δE k-1 are respectively the position, velocity, heading angle of the robot and the sensor measurement error at the previous moment, δt is the sampling interval time, γ od is the autocorrelation time constant of the encoder speed measurement scale factor, is the corresponding variance; β z is the autocorrelation time constant of the gyroscope random drift, is the corresponding variance.

[0043] Further, the lidar point cloud distance range in step 5 is [0.03m, 0.3m], the angle range is and the acquisition frequency is 30Hz.

[0044] Further, in step 6, extracting the straight-line feature based on the lidar point cloud intensity and density to calculate and solve the robot's heading angle and lateral offset is specifically as follows:

[0045] Step 6-1: For the point cloud processed by de-distortion in step 3, select the point p with the maximum reflection intensity;

[0046] Step 6-2: Calculate the distance attributes of the remaining points and point p, specifically

[0047]

[0048] Among them, the vectors x, y, and inten are the normalized coordinates and intensity of the point cloud;

[0049] Step 6-3: Compare this property with the distance threshold ∈. If there are at least minpts points around point p, cluster these points into one class. Specifically, the parameters ∈ and minpts are set to 0.02 m and 10 respectively;

[0050] Step 6-4: Repeat steps 4-1 to 4-3 for the remaining points until all points have been discriminated;

[0051] Step 6-5: For this class of points that have completed clustering, randomly select two points p1 and p2;

[0052] Step 6-6: Calculate the linear model parameters m and b through these two points p1 and p2;

[0053] Step 6-7: Calculate the error function of the distances of the remaining points in this class with respect to this line, specifically:

[0054]

[0055] In the formula, M is the number of remaining points, x n , y n are the abscissa and ordinate of this point, and m and b are the parameters of this line;

[0056] Step 6-8: Randomly select two points again and repeat steps 4-5 to 4-7 until all points have been selected;

[0057] Step 6-9: Select a group of points with the minimum error function value and calculate the linear model parameters as the extracted linear feature m best , b best ;

[0058] Step 6-10: Solve the line feature to calculate the robot's orientation angle and lateral offset, specifically:

[0059] θ Lidar = θ o - arctan m

[0060]

[0061] where, θ Lidar , y Lidar are the calculated robot's orientation angle and lateral offset, and θ0 and y0 are determined by initial alignment.

[0062] Furthermore, in step 7, update the Kalman gain, the robot system state and covariance from the measurement values, and calculate the robot pose specifically as follows:

[0063] Step 7-1: Construct the system error measurement model as:

[0064]

[0065] wherein, δZ k is the difference between the pose calculated by the lidar in step 6 and the pose estimated by the robot dynamics model, and are respectively the y-axis coordinate of the robot calculated by the lidar in the navigation coordinate system and the y-axis coordinate of the robot calculated by the robot system dynamics model in the navigation coordinate system, and are respectively the orientation angle of the robot calculated by the lidar in the navigation coordinate system and the orientation angle of the robot calculated by the robot system dynamics model in the navigation coordinate system,

[0066] is the set observation matrix, δX k is the system error state of the robot, δν k is the measurement error vector set as Gaussian white noise.

[0067] Step 7-2: Update the Kalman gain, the robot system state and the covariance according to the system error measurement value.

[0068] Compared with the prior art, the remarkable progress of the present invention lies in: 1) The present invention is a completely autonomous positioning method and does not require additional devices such as GPS, UWB, infrasound, etc.; 2) The present invention extracts features from the point cloud to estimate the pose of the pipeline robot. Compared with the traditional feature extraction method, the present invention significantly reduces the distortion phenomenon of the point cloud due to material reflection according to the material characteristics of the ventilation pipeline. At the same time, the feature extraction method of the present invention significantly reduces the running time and resource consumption and can be applied to an embedded robot; 3) The present invention can achieve good results in a complex pipeline environment background, which is beneficial to eliminating the cumulative error of self-positioning of the pipeline robot in a complex pipeline environment and improving the positioning accuracy of the pipeline robot.

[0069] To more clearly illustrate the functional characteristics and structural parameters of the present invention, the following further illustrates in conjunction with the drawings and specific embodiments. Brief Description of the Drawings

[0070] The drawings described herein are used to provide a further understanding of the present invention and constitute a part of this application. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation to the present invention. In the drawings:

[0071] Figure 1 is the schematic diagram of a lidar-assisted real-time positioning method for a pipeline inspection robot based on EKF. Detailed Description of the Invention

[0072] Next, in combination with the accompanying drawings in the embodiments of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts belong to the scope of protection of the present invention.

[0073] As Figure 1 shown, a laser radar-assisted real-time positioning method for a pipeline inspection robot based on EKF includes the following steps:

[0074] Step 1: Establish a dynamic model of the ventilation pipeline inspection robot system;

[0075] Step 2: Place the robot in the ventilation pipeline to collect point cloud data and manually annotate it, and train an artificial neural network of the reflection distortion model of the lidar point cloud with respect to the ventilation pipeline wall;

[0076] Step 3: Initialize the robot state and covariance at the starting working point;

[0077] Step 4: Predict the state vector and covariance through the system equation;

[0078] Step 5: Collect point cloud data through the lidar installed on the side of the robot, and perform distortion removal processing on the distorted point cloud in the ventilation pipeline environment based on the reflection distortion correction artificial neural network;

[0079] Step 6: Extract line features based on the lidar point cloud intensity and density to perform inverse calculation to obtain the robot's orientation angle and lateral offset;

[0080] Step 7: Update the Kalman gain, robot system state, and covariance from the measurement values.

[0081] Specifically, in this embodiment, for the establishment of the dynamic model of the ventilation pipeline inspection robot system in Step 1, the specific steps are as follows:

[0082] Step 1-1: Establish a sensor model of the pipeline robot gyroscope and encoder:

[0083]

[0084] Among them, ω is the angular velocity of the pipeline robot, ω z is the output angular velocity of the robot gyroscope, b z is the gyroscope zero bias, v x and v y are the velocities of the robot along the x-axis and y-axis in the navigation coordinate system respectively, and v and θ are the encoder output velocity and heading angle of the robot in the navigation coordinate system;

[0085] Step 1-2: Establish the dynamic model of the robot system by using the sensor models of the robot gyroscope and encoder as follows:

[0086]

[0087] where the subscript k and the subscript k-1 represent the sampling moments, δt represents the sampling time interval, and x k , y k , v xk , v yk , θ k respectively represent the coordinate of the robot along the x-axis direction, the coordinate along the y-axis direction, the velocity along the x-axis direction, the velocity along the y-axis, and the orientation angle at the k-th moment in the navigation coordinate system.

[0088] Specifically, in this embodiment, in Step 2, placing the robot in the ventilation duct to collect point cloud data and manually annotating it, and training the artificial neural network of the reflection distortion model of the lidar point cloud with respect to the ventilation duct wall, the specific steps are as follows:

[0089] Step 2-1: Place the robot in the working environment, and recombine the distance attribute of the collected point cloud data with the point cloud intensity and angle after manually annotating and modifying it according to the true value of manual measurement to construct the training set t;

[0090] Step 2-2: Construct a fully connected network M with a multi-layer perceptron structure, specifically:

[0091]

[0092] where the vectors x and y are the input layer and output layer of the network respectively. The x vector is composed of the angle and intensity of the point cloud, the y vector represents the distance attribute of the point cloud, and the features of the network are represented by the weights υ j , ω i , the bias b i , λ and the activation functions σ, ρ parameters, where the activation functions are the tansig function and the purelin function respectively;

[0093] Step 2-3: Calculate the error between the output result of the network M and the true value according to the loss function J, and iteratively train the network parameters, specifically:

[0094]

[0095] where N is the number of point clouds, y n is the distance of the output point cloud of the network, and t n is the distance value of the point cloud in the data set.

[0096] Specifically, in this embodiment, in Step 3, initialize the robot state and covariance at the starting working point, and the specific steps are as follows:

[0097] Define the error state vector between the dynamic state and the true state of the robot system as:

[0098]

[0099] where δx and δy are the position coordinate errors of the robot in the navigation coordinate system; δv x and δv y are the velocity component errors of the robot along the x-axis and y-axis in the navigation coordinate system; δθ, δb od and δb z are the heading angle error, the encoder cumulative error, and the gyroscope drift error, respectively; the system covariance is determined by the performance of the encoder and gyroscope assembled on the robot.

[0100] Specifically, in this embodiment, predicting the error state vector and covariance through the system equation in step 4 is specifically:

[0101]

[0102] where

[0103]

[0104]

[0105]

[0106]

[0107] In the formula, γ od is the autocorrelation time constant of the encoder speed measurement scale factor, is the corresponding variance; β z is the autocorrelation time constant of the gyroscope random drift, is the corresponding variance.

[0108] Specifically, in this embodiment, the range of the lidar point cloud distance in step 5 is [0.03m, 0.3m], and the angle range is The acquisition frequency is 30Hz.

[0109] Specifically, in this embodiment, extracting the straight line feature based on the lidar point cloud intensity and density to calculate the robot's heading angle and lateral offset in step 6 is specifically:

[0110] Step 6-1: For the point cloud processed by de-distortion in step 3, select the point p with the maximum reflection intensity;

[0111] Step 6-2: Calculate the distance attributes between the remaining points and the point p, specifically

[0112]

[0113] Among them, the vectors x, y, and inten are the normalized coordinates and intensity of the point cloud respectively;

[0114] Step 6-3: Compare this property with the distance threshold ∈. If there are at least minpts points around point p, then cluster these points into one class. Specifically, the parameters ∈ and minpts are set to 0.02 m and 10 respectively;

[0115] Step 6-4: Repeat Steps 4-1 to 4-3 for the remaining points until all points have been discriminated;

[0116] Step 6-5: For this class of points that have completed clustering, randomly select two points p1 and p2;

[0117] Step 6-6: Calculate the linear model parameters m and b through these two points p1 and p2;

[0118] Step 6-7: Calculate the error function of the distances of the remaining points in this class with respect to this line. Specifically:

[0119]

[0120] Step 6-8: Randomly select two points again and repeat Steps 4-5 to 4-7 until all points have been selected;

[0121] Step 6-9: Select a group of points with the minimum error function value and calculate the linear model parameters as the extracted line features m best , b best ;

[0122] Step 6-10: Solve the line feature to calculate the robot's orientation angle and lateral offset. Specifically:

[0123] θ Lidar = θ0 - arctan m

[0124]

[0125] Among them, θ Lidar , y Lidar are the calculated robot's orientation angle and lateral offset, and θ0 and y0 are determined by the initial alignment.

[0126] Specifically, in this embodiment, in Step 7, updating the Kalman gain, the robot system state, and the covariance from the measurement values and calculating the robot pose are specifically as follows:

[0127] Step 7-1: Construct the system error measurement model as:

[0128]

[0129] Among them, δZ k is the difference between the pose calculated by the lidar in step 6 and the pose estimated by the robot dynamics model, is the set observation matrix, and ν k is the measurement error vector set as Gaussian white noise.

[0130] Step 7-2: Update the Kalman gain, the robot system state, and the covariance based on the system error measurement value.

[0131] It should be noted that in this article, relational terms such as first and second are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the term "comprising", "including" or any other variant thereof is intended to cover non-exclusive inclusion, so that a process, method, article or device comprising a series of elements not only includes those elements, but also includes other elements not expressly listed, or further includes elements inherent to such process, method, article or device.

[0132] Although the embodiments of the present invention have been shown and described, it will be understood by those of ordinary skill in the art that various changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and spirit of the present invention, and the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A method for real-time positioning assisted by lidar of a pipeline inspection robot based on EKF, characterized in that, It includes the following steps: Step 1: Establish the dynamic model of the ventilation duct inspection robot system; Step 2: Place the robot in the ventilation duct to collect point cloud data and manually annotate it, and train the artificial neural network of the reflection distortion model of the lidar point cloud with respect to the ventilation duct wall; Step 3: Initialize the robot state and covariance at the starting working point; Step 4: Predict the state vector and covariance through the system equation; Step 5: Collect point cloud data through the lidar installed on the side of the robot, and perform distortion removal on the distorted point cloud in the ventilation duct environment based on the reflection distortion correction artificial neural network; Step 6: Extract the straight line features based on the lidar point cloud intensity and density and perform inverse calculation to calculate the robot's orientation angle and lateral offset; Step 7: Update the Kalman gain, the robot system state and covariance from the measurement values.

2. A method for real-time positioning assisted by lidar of a pipeline inspection robot based on EKF according to claim 1, characterized in that The specific steps for establishing the dynamic model of the ventilation duct inspection robot system in Step 1 are as follows: Step 1-1: Establish the sensor models of the pipeline robot's gyroscope and encoder: where ω is the angular velocity of the pipeline robot, ω z is the angular velocity output by the robot's gyroscope, b z is the gyroscope zero bias, v x and v y are the velocities of the robot along the x-axis and y-axis in the navigation coordinate system respectively, and v and θ are the encoder output velocity and heading angle of the robot in the navigation coordinate system; Step 1-2: Use the sensor models of the robot's gyroscope and encoder to establish the dynamic model of the robot system as: where the subscript k and the subscript k - 1 represent the sampling times, δt represents the sampling time interval, and x k , y k , θ k represent the coordinate in the x-axis direction, the coordinate in the y-axis direction, the velocity in the x-axis direction, the velocity in the y-axis direction, and the orientation angle of the robot at the k-th moment in the navigation coordinate system, respectively.

3. A method for real-time positioning assisted by lidar of a pipeline inspection robot based on EKF according to claim 1, characterized in that, The specific steps for placing the robot in the ventilation duct to collect point cloud data and manually annotate it, and training the artificial neural network of the reflection distortion model of the lidar point cloud with respect to the ventilation duct wall in Step 2 are as follows: Step 2-1: Place the robot in the working environment, modify the distance attribute of the collected point cloud data according to the manually measured true value, and recombine it with the point cloud intensity and angle to construct the training set t; Step 2-2: Construct a fully connected network M with a multi-layer perceptron structure, specifically: Among them, vectors x and y are the input layer and output layer of the network respectively. The x vector is composed of the angle and intensity of the point cloud, and the y vector represents the distance attribute of the point cloud. The features of the network are represented by weights υ j , ω i , bias b i , λ, and activation functions σ and ρ parameters, where the activation functions are the tansig function and the purelin function respectively; Step 2-3: Calculate the error between the output result of network M and the true value according to the loss function J, and iteratively train the network parameters, specifically: Among them, N is the number of point clouds, and y n is the distance of the point cloud output by the network, and t n is the distance value of the point cloud in the dataset.

4. A method for real-time positioning assisted by lidar of a pipeline inspection robot based on EKF according to claim 1, characterized in that, The specific steps for initializing the robot state and covariance at the starting working point in Step 3 are as follows: Define the error state vector between the dynamic state of the robot system and the true state as: where, δX is the defined error state vector, and δP, δV, δE are the robot's position, velocity, heading angle and sensor measurement errors respectively. Specifically: δx and δy are the position coordinate errors of the robot in the navigation coordinate system; δv x and δv y are the velocity component errors of the robot along the x-axis and y-axis in the navigation coordinate system; δθ, δb od and δb z are the heading angle error, the encoder cumulative error and the gyroscope drift error respectively; the system covariance is determined by the performance of the encoder and gyroscope assembled on the robot, and T is the matrix transpose symbol.

5. A method for real-time positioning of a pipeline inspection robot assisted by lidar based on EKF according to claim 1, characterized in that, The specific steps for predicting the error state vector and covariance through the system equation in Step 4 are as follows: where, where, I 2×2 is a 2nd order identity matrix, 0 2×2 , 0 2×3 and 0 3×2 are corresponding zero matrices, δP k-1 , δV k-1 , δE k-1 are the robot position, velocity, heading angle and sensor measurement error at the previous moment respectively, δt is the sampling interval time, γ od is the autocorrelation time constant of the encoder speed measurement scale factor, is the corresponding variance; β z is the autocorrelation time constant of the gyro random drift, is the corresponding variance.

6. A method for real-time positioning of a pipeline inspection robot assisted by lidar based on EKF according to claim 1, characterized in that The lidar point cloud distance range in step 5 is [0.03m, 0.3m], and the angle range is The acquisition frequency is 30Hz.

7. A method for real-time positioning of a pipeline inspection robot assisted by lidar based on EKF according to claim 1, characterized in that, The specific steps for calculating the robot's orientation angle and lateral offset by extracting the straight line features based on the lidar point cloud intensity and density in Step 6 are as follows: Step 6-1: For the point cloud processed by distortion removal in Step 3, select the point p with the maximum reflection intensity; Step 6-2: Calculate the distance attributes of the remaining points from point p, specifically where, the vectors x, y, inten are the normalized coordinates and intensity of the point cloud; Step 6-3: Compare this attribute with the distance threshold ∈. If there are at least minpts points around point p, then cluster these points into one category. Specifically, the parameters ∈ and minpts are set to 0.02m and 10 respectively; Step 6-4: Repeat Steps 4-1 to 4-3 for the remaining points until all points have been discriminated; Step 6-5: For this category of points that have completed clustering, randomly select two points p1 and p2; Step 6-6: Calculate the linear model parameters m and b through these two points p1 and p2; Step 6-7: Calculate the error function of the distances of the remaining points in this class with respect to this line, specifically: where M is the number of remaining points, x n , y n are the abscissa and ordinate of the point, and m and b are the parameters of the straight line; Step 6-8: Randomly select two points again, and repeat Steps 4-5 to 4-7 until all points are selected; Step 6-9: Select a set of points with the minimum error function value, and calculate the linear model parameters as the extracted linear feature m best , b best ; Step 6-10: Solve the line feature to calculate the robot's orientation angle and lateral offset, specifically: θ Lidar = θ0 - arctan m where θ Lidar , y Lidar are the calculated robot orientation angle and lateral offset, and θ0, y0 are determined by initial alignment.

8. A method for real-time positioning assisted by lidar of a pipeline inspection robot based on EKF according to claim 1, characterized in that, In Step 7, update the Kalman gain, the robot system state, and the covariance from the measurement values, and calculate the robot pose specifically: Step 7-1: Construct the system error measurement model as: where, δZ k is the difference between the pose calculated by the lidar in step 6 and the pose estimated by the robot dynamics model, and are the y-axis coordinates of the robot calculated by the lidar and the y-axis coordinates of the robot calculated by the robot system dynamics model in the navigation coordinate system, respectively, and are the orientation angles of the robot calculated by the lidar and the orientation angles of the robot calculated by the robot system dynamics model in the navigation coordinate system, respectively, is the set observation matrix, δX k is the system error state of the robot, δν k is the measurement error vector set as Gaussian white noise; Step 7-2: Update the Kalman gain, the robot system state, and the covariance from the system error measurement values.

Images