Underwater pipeline robot positioning method

Abstract

The invention discloses an underwater pipeline robot positioning method, which comprises the following steps of: firstly, acquiring an initial course angle and a position by using an inertial measurement unit (IMU), and establishing a Kalman filtering state model; thirdly, calculating the arc length change by combining the accumulated rotation angle of the encoder wheel with the course angle change measured by the IMU and the radius of the pipeline, and updating the state by taking the arc length change as an observation value; and analyzing and identifying a slip event by adopting the relationship between the speed variation and the observation distance variation. Optimal process noise matrix and observation noise matrix parameters in normal and slip states are generated in advance through off-line Bayesian optimization, and when slip is detected, the parameters are switched to corresponding parameters, and arc length variation compensation errors of an encoder and an IMU (Inertial Measurement Unit) are fused. And finally, the high-precision arc length position and the course angle are output, and accurate positioning of the position, relative to the initial state, of the robot on the pipeline is achieved. According to the invention, the navigation and positioning precision of the underwater robot in a complex environment is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater robot navigation and positioning, and particularly relates to a positioning method for an underwater pipeline robot. Background Technique

[0002] In the fields of ocean engineering (such as ocean platforms, offshore wind power), water-crossing bridges, nuclear power pressure vessels, etc., the underwater structures are mostly cylindrical structures, and the safe operation and maintenance of these structures are directly related to energy security, environmental protection, and public safety. The traditional manual detection method has low efficiency and high risk, while the underwater robot detection technology has become the mainstream solution due to its high efficiency and safety.

[0003] For this, the existing underwater robot positioning methods mainly include various methods such as inertial navigation, sonar positioning, and imaging positioning. Currently, the commonly used positioning methods during underwater navigation mainly include an inertial navigation system (Inertial Navigation System, INS), underwater acoustic positioning technology, Doppler velocity log (DVL), etc., or use the above methods combined with an extended Kalman filter algorithm (Extended Kalman Filter, EKF), etc. to achieve the positioning of a single sensor node.

[0004] However, the above methods cannot be used for the positioning of millimeter-level welds or defects on the surface of underwater cylindrical structures. According to the current requirements for the detection accuracy of weld defects, for area-shaped defects, the error in measuring its length and width does not exceed ±5 mm, and the accuracy requirements are higher in the detection fields such as nuclear power pressure vessels. The existing multi-sensor fusion of an odometer and an imu is mainly used for onshore positioning, and the Kalman filter with fixed noise parameters (Q, R matrices) is used. Due to the particularity of the underwater pipeline structure, the robot is prone to slipping, resulting in the inability to adapt to a dynamic environment (such as sudden slipping, running over protrusions), resulting in model mismatch. The positioning technology without pertinence often cannot meet the high-precision positioning requirements.

[0005] Therefore, it is very important to develop a positioning method that integrates the positioning technology for an adsorption-type underwater robot on the surface of a cylindrical structure. Although the underwater robot positioning technology has been relatively well developed over the years, the research results in the positioning technology to deal with the easy slipping of pipeline robots are still very limited.

[0006] For an adsorption - type underwater structure detection robot, the problem of inaccurate positioning caused by easy slipping in the working environment is still a pain point. Problems such as weak light, turbid water flow, and electromagnetic signal attenuation limit the reliability of sensors such as vision and acoustics, resulting in a decline in environmental perception ability. Complex structures such as bending, branching, variable diameter, or corrosion deformation increase the difficulty of motion trajectory modeling. Traditional positioning algorithms are prone to cumulative errors. Factors such as biological attachment, anti - corrosion coatings, and corrosion layers reducing the wheel - wall friction coefficient further lead to an increase in the slipping rate of the driving wheel in vertical pipes. Due to limitations such as static noise models, empirical threshold slipping detection, and fixed - weight data fusion in traditional positioning technologies, it is difficult to cope with the above challenges. Summary of the Invention

[0007] An object of an embodiment of the present invention is to provide a positioning method for an underwater pipeline robot, which solves the problems of insufficient accuracy, easy slipping, and cumulative errors in traditional positioning technologies during weld or defect detection, and realizes high - precision and reliable detection of surface defects of underwater structures.

[0008] To solve the above - mentioned technical problems, the technical solution adopted by the present invention is a positioning method for an underwater pipeline robot, which is specifically carried out according to the following steps:

[0009] S1. Based on the heading angle data, angular velocity data of the underwater pipeline robot collected by the inertial measurement unit, as well as the rotation angle data and angular velocity data of the encoder wheel, and the data of the publishing node and receiving node of the rotation angle data and angular velocity data of the encoder wheel, establish a Kalman filter state model; at the same time, calculate the change in the moving arc length of the pipeline robot according to the change in the heading angle collected by the inertial measurement unit and in combination with the pipeline radius, and use the change in the moving arc length of the pipeline robot as an observation value for state update;

[0010] S2. Synchronize the data of the inertial measurement unit and the encoder wheel, analyze and identify whether the underwater pipeline robot slips by using the dynamic relationship between the linear velocity difference and the change in the observed distance, and perform compensation by using the change in the arc length between the encoder and the synchronized inertial measurement unit;

[0011] S3. Perform off - line Bayesian optimization to train the noise matrix Q and the observation noise matrix R parameters during the slipping and non - slipping processes of the underwater pipeline robot;

[0012] S4. After the slipping is triggered, switch to the optimal process noise matrix Q and observation noise matrix R during the pre - training process, update the process noise matrix Q and the observation noise matrix R, and then output the high - precision arc length position and heading angle to complete the positioning of the underwater pipeline robot.

[0013] Furthermore, the specific process of establishing the Kalman filter state model in S1 is as follows:

[0014] S101. Determine the moving arc length of the underwater pipeline robot on the pipeline:

[0015] L e = θ e ·πr / 180°

[0016] L i = α i ·πR p / 180°

[0017] Wherein, L e is the arc length observed by the encoder, L i is the arc length observed by the inertial measurement unit, θ e is the cumulative rotation angle of the encoder wheel, α i is the heading angle information directly obtained by the inertial measurement unit, r is the radius of the encoder wheel, R p is the radius of the pipeline;

[0018] S102. Establish a Kalman filter state model:

[0019]

[0020] Wherein, X k is the state vector at time k, A is the state transformation matrix, X k-1 is the state vector at time k - 1, W k , V k are independent white noise matrices that follow a normal distribution, N represents the normal distribution, Z k is the observation vector at time k, is the process excitation noise covariance matrix, and R is the observation noise covariance matrix;

[0021] Among them, the state vector is:

[0022] X = [s v]T

[0023] Where s is the moving arc length of the underwater pipeline robot on the pipeline, and v is the linear velocity of the underwater pipeline robot;

[0024] The state transformation matrix A is:

[0025]

[0026] Where Δt is the sensor data reception frequency;

[0027] The observation matrix H is:

[0028]

[0029] S103. Predict and correct the Kalman filter state model.

[0030] Further, the prediction in S103 is to establish a prior estimate of the current state using the time update equation, and timely calculate forward the values of the current state variables and the error covariance estimate to construct a prior estimate value for the next time state;

[0031] The correction is to establish an improved posterior estimate of the current state based on the prior estimate value in the prediction process and the current measurement variable using the measurement update equation;

[0032] Among them, the prior error E k - and the posterior error E k are respectively:

[0033]

[0034] The posterior error covariance matrix P k - and the prior error covariance matrix P k are respectively:

[0035]

[0036] The time update equation is:

[0037]

[0038] The state update equation is:

[0039]

[0040] Among them, is the state estimate at time k; is the state estimate at time k-1, is the prior state estimate at time k; A is the state transformation matrix; Q is the process excitation noise covariance matrix; I is the identity matrix; K k is the Kalman gain, R is the observation noise covariance matrix; H is the observation matrix; T is the transpose symbol; E is the expected variance matrix.

[0041] Further, the specific steps of S2 are as follows:

[0042] S201. Use the message_filters package in the ROS system to synchronize the received timestamps of the encoder wheel and the inertial measurement unit data receivers;

[0043] S202. After synchronizing the timestamps, obtain the moving distance provided by the encoder and the change in moving distance ΔL provided by the inertial measurement unit e and ΔL i are:

[0044] ΔLe = Δθ e · πr / 180°

[0045] ΔL i = Δα i · πR p / 180°

[0046] Where, Δθ e is the difference in the cumulative rotation angle of the encoder between the current moment and the previous moment; Δα i is the difference in the heading angle recorded by the inertial measurement unit between the current moment and the previous moment, r is the radius of the encoder wheel, and R p is the radius of the pipeline;

[0047] S203. Determine the linear velocity provided by the encoder wheel and the linear velocity provided by the inertial measurement unit:

[0048] V e = ω e · r

[0049] V i = V z · R p

[0050] Where, V e is the linear velocity provided by the encoder wheel, ω e is the angular velocity of the encoder rotation, r is the radius of the encoder wheel, V i is the linear velocity provided by the inertial measurement unit, V z is the angular velocity of rotation about the z-axis recorded by the inertial measurement unit, and R p is the radius of the pipeline;

[0051] S204. Detect whether the underwater pipeline robot is slipping based on the differences in velocity and distance change;

[0052] S205. When slipping is detected, adjust the estimated value of the robot state by the cumulative error compensation method.

[0053] Furthermore, the specific process of S204 is as follows: Use a sliding window to process the data sequences of the inertial measurement unit and the encoder wheel. Within the sliding window, if the difference in the distance change recorded by the encoder wheel and the inertial measurement unit is too large, and there is also a large difference in the linear velocity, then the encoder wheel is in a slipping state. The specific condition is: ΔL e - △L i > 3·σ △s + 0.1 and |V e - V i | > 3·σ v ; Where, σ Δs and σ vSpecifically, they are the standard deviations within the time windows of the distance change amount and the speed change amount, V e is the linear velocity provided to the encoder wheel, ΔL e and ΔL i are the moving distance change amounts provided by the encoder wheel and the inertial measurement unit respectively at the slipping moment.

[0054] Furthermore, the specific method of adjusting the estimated value of the robot state by the cumulative error compensation method in S205 is as follows:

[0055] L = L e -ΔE

[0056] ΔE = ∑(ΔL e -ΔL i )

[0057] where L is the observed arc length of the encoder wheel after compensation, L e is the observed arc length of the encoder wheel at the slipping moment, ΔE is the cumulative slipping error, ΔL e , ΔL i are the moving distance change amounts provided by the encoder wheel and the inertial measurement unit respectively at the slipping moment; if there is no slipping, no processing is performed on L e , and E represents the expected variance matrix.

[0058] Furthermore, the specific process of S3 is as follows:

[0059] S301. After completing the construction of the Kalman filter state model and realizing the encoder wheel slipping detection and compensation, use the offline Bayesian optimization based on Gaussian process to obtain the process noise and observation noise when there is no slipping and when there is slipping, so that the difference between the data fusion result and the true moving distance is minimized. The objective function is:

[0060] x * = arg min f(x)

[0061] where x * is the d-dimensional hyperparameter that optimizes the model generalization performance in the hyperparameter space, and f(x) is the mapping from the hyperparameter vector x to the model generalization performance;

[0062] S302. Use the Gaussian process function to fit f(x);

[0063] S303. Use the expected improvement covariance function α EI (x) as the acquisition function;

[0064] S304. Perform Bayesian optimization with 100 iterations on the underwater pipeline robot non-slipping dataset and the slipping dataset collected respectively, and obtain the optimal process noise and observation noise matrix parameters when there is no slipping as:

[0065]

[0066] The optimal process noise and observation noise matrix parameters after slipping are as follows:

[0067]

[0068] Furthermore, the specific process of S4 is as follows: Dynamically adjust the optimal process noise matrix and observation noise matrix parameters in the slipping and non-slipping states based on the detected slipping state of the underwater pipeline robot; in the case of no detected slipping, continue to use the optimized process noise matrix and observation noise matrix parameters in the non-slipping state. When more than two consecutive slipping signals are detected, switch to the optimal process noise matrix and observation noise matrix parameters after slipping obtained by training in S3, and output the high-precision distance of the underwater pipeline robot moving relative to the initial position.

[0069] Compared with the prior art, the beneficial effects of the present invention are as follows: The present invention can achieve a high-precision positioning error within ±3 mm in the case of no slipping, ensuring the precise positioning ability of the robot in a complex environment. The detection accuracy of the present invention for slipping events exceeds 95%, significantly improving the reliability of the system, and can timely adjust the positioning strategy when slipping occurs, ensuring the continuity and accuracy of navigation and positioning.

[0070] The Kalman filter state model adopted by the present invention effectively compensates for the change in arc length by fusing the data of the encoder and the IMU. Even in the case of slipping of the encoder wheel, the present invention can still maintain a high positioning accuracy, and the performance of the method of the present invention is superior to that of using only the IMU for positioning.

[0071] The present invention only relies on two sensors (IMU and encoder wheel), occupies less computing resources, has a fast response speed, and has good real-time performance. The present invention not only simplifies the system design, but also reduces the hardware cost and improves the data processing efficiency. In summary, the present invention provides an efficient and reliable technical solution for the navigation and positioning of underwater robots, especially suitable for the field of underwater structure detection, showing a broad application prospect. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0073] Figure 1It is the flow chart of the multi-sensor fusion algorithm of the present invention;

[0074] Figure 2 It is the position comparison diagram of the predicted trajectory and the positioning using only the encoder wheel and the imu in the embodiment of the present invention;

[0075] Figure 3 It is the comparison diagram of the positioning error without slipping and the positioning error using only the encoder wheel and the imu in the embodiment of the present invention;

[0076] Figure 4 It is the comparison diagram of the positioning error with slipping and the positioning error using only the encoder wheel and the imu in the embodiment of the present invention. Detailed implementation manners

[0077] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. 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 shall fall within the protection scope of the present invention.

[0078] Such as Figure 1 , this embodiment provides an underwater pipeline robot positioning method, which integrates an encoder wheel and an IMU, and in view of the phenomenon of easy slipping in the special operation scenario of the underwater pipeline robot, the optimal Kalman filter noise matrix parameters in two cases are obtained by Bayesian optimization, dynamically switched according to whether slipping occurs, and data compensation is performed to provide more credible positioning information. This embodiment uses a dynamic switching Kalman filter residue underwater robot pipeline positioning method to make up for the problem of different positioning accuracies when slipping occurs.

[0079] In some specific implementation manners, the underwater pipeline robot positioning method includes the following steps:

[0080] S1. Use an inertial measurement unit (IMU) to determine the real-time data of the pipeline robot, so as to obtain the initial position of the robot in the pipeline. Based on this, a Kalman filter state model is established, and the cumulative rotation angle of the encoder wheel is used, and the high-frequency displacement increment provided by the encoder wheel is used to drive the prediction stage. At the same time, according to the change in the heading angle measured by the IMU and combined with the pipeline radius, the change in the moving arc length of the pipeline robot is calculated, and the calculation result is used as an observation value for state update.

[0081] In some specific implementation manners, the specific establishment process of the Kalman filter state model is as follows:

[0082] Under the ROS system, establish the imu heading angle data, angular velocity data, encoder wheel rotation angle data, angular velocity data, and the data of the publishing node and receiving node. After obtaining the above real-time data, establish a Kalman filter state model. Use the cumulative rotation angle of the encoder wheel combined with the encoder wheel radius to infer the arc length change measured by the encoder, and use the change in the imu measured heading angle combined with the pipeline radius to calculate the arc length change measured by the imu. Use the calculation results as the observation value for state update. Specifically:

[0083] S101. Based on the heading angle recorded by the imu and the cumulative rotation angle of the encoder wheel, respectively obtain the arc lengths that the robot moves on the pipeline as follows:

[0084] L e = θ e ·πr / 180°

[0085] L i = α i ·πR p / 180°

[0086] Among them, L e is the arc length observed by the encoder wheel, L i is the arc length observed by the imu, θ e is the cumulative rotation angle of the encoder wheel, α i is the heading angle information directly obtained by the imu, r is the radius of the encoder wheel, and R p is the radius of the pipeline. The angular accuracy of the encoder wheel is 0.01°, and the heading angle accuracy of the imu is 0.5°.

[0087] S102. Establish a Kalman filter state model:

[0088]

[0089] Among them, X k is the state vector at time k, A is the state transformation matrix, X k-1 is the state vector at time k - 1, W k , V k are independent white noise matrices that follow a normal distribution, N represents the normal distribution, Z k is the observation vector at time k, is the process excitation noise covariance matrix, and R is the observation noise covariance matrix.

[0090] The state vector is:

[0091] X = [s v] T

[0092] Among them, s is the arc length that the robot moves on the pipeline, and v is the linear velocity of the underwater pipeline robot;

[0093] The state transition matrix A is as follows:

[0094]

[0095] where Δt = 0.01 s, which is the sensor data reception frequency;

[0096] The observation matrix H is:

[0097]

[0098] The upper left and lower left elements in the matrix respectively represent the imu observed arc length and the encoder wheel observed arc length.

[0099] S103. Estimate and correct the Kalman filter state model. In the estimation process, the time update equation is used to establish a prior estimate of the current state, and the values of the current state variables and error covariance estimates are timely calculated forward to construct a prior estimate value for the next time state; the correction process is responsible for feedback, and the measurement update equation is used to establish an improved posterior estimate of the current state based on the prior estimate value in the estimation process and the current measurement variables. Finally, the optimal estimated arc length is output, and the selection of the observation noise and process noise values is given in the subsequent steps.

[0100] Among them, the prior error E k - and the posterior error E k are as follows:

[0101]

[0102]

[0103] The prior error covariance matrix and the posterior error covariance matrix are:

[0104]

[0105] where E represents the expected variance matrix;

[0106] The time update equation is:

[0107]

[0108] The state update equation is:

[0109]

[0110] In the above formula, the declarations of each variable are as follows:

[0111] is the state estimate at time k; is the state estimate at time k - 1; A is the state transition matrix; Q is the process excitation noise covariance matrix;

[0112] The n×n posteriori estimation error covariance matrix; K k : The n×m Kalman gain, R: The observation noise covariance matrix; I: The n×n identity matrix.

[0113] S2. Synchronize the imu and encoder wheel data, analyze and identify whether the underwater robot has a slipping event by using the dynamic relationship between the linear velocity difference and the observed distance change, and compensate by using the arc length change between the encoder wheel and the IMU; specifically:[[]]

[0114] S201. Use the receiver of the message_filters package in the ROS system to synchronize the receiving timestamps of the encoder wheel and imu data.

[0115] S202. After synchronizing the timestamps, obtain the moving distance provided by the encoder wheel and the moving distance change provided by the imu as follows:

[0116] ΔL e =△θ e ·πr / 180°

[0117] ΔL i =Δα i ·πR p / 180°

[0118] Where, Δθ e is the difference in the cumulative rotation angle of the encoder wheel between the current moment and the previous moment; Δα i is the difference in the heading angle recorded by the imu between the current moment and the previous moment, r is the radius of the encoder wheel, and R p is the radius of the pipeline.

[0119] S203. Determine the linear velocity provided by the encoder wheel and the linear velocity provided by the inertial measurement unit:

[0120] V e =ω e ·r

[0121] V i =V z ·R p

[0122] Where, V e is the linear velocity provided by the encoder wheel, w e is the rotational angular velocity of the encoder wheel, and r is the radius of the encoder wheel. V i is the linear velocity provided by the IMU, and V z is the angular velocity of rotation around the z-axis recorded by the imu.

[0123] S204. Detect slipping based on the differences in speed and distance change. Process the sensor data sequence using a sliding window. Within the sliding window, if the difference in the distance change recorded by the encoder wheel and the IMU is too large, and there is also a large difference in the linear velocity, then the wheel is in a slipping state. The specific conditions are: ΔL e -ΔL i >3·σ Δs +0.1 and |V e -V i |>3·σ v

[0124] where σ Δs and σ v are the standard deviations within the time window of the distance change and the speed change respectively. A threshold of 0.1 is set to avoid misjudgment caused by the time delay in sensor data publication when the robot starts moving;

[0125] S205. After detecting slipping, adjust the estimated value of the robot state through the cumulative error compensation method:

[0126] L = L e -ΔE

[0127] ΔE = ∑(ΔL e -ΔL i )

[0128] where L is the observed arc length of the encoder wheel after compensation, L e is the observed arc length of the encoder wheel at the slipping moment, ΔE is the cumulative slipping error, and ΔL e , ΔL i are the moving distance changes provided by the encoder wheel and the imu at the slipping moment respectively. If there is no slipping, L e is not processed.

[0129] S3. Generate two sets of process noise matrix Q and observation noise matrix R parameters for slipping / non-slipping through offline Bayesian optimization;

[0130] S301. After successfully constructing the Kalman filter state model and realizing encoder wheel slipping detection and compensation, use offline Bayesian optimization based on Gaussian process to obtain two sets of process noise and observation noise for non-slipping / slipping, so that the difference between the data fusion result and the true moving distance is minimized. The objective function formula is as follows:

[0131] x * =arg min f(x)

[0132] where x *is the d-dimensional hyperparameter that optimizes the model generalization performance within the hyperparameter space. f(x) is the mapping from the hyperparameter vector x to the model generalization performance and is a black-box objective function with high evaluation cost.

[0133] S302. Use the Gaussian process function to fit f(x). Given that the observed data D contains (x1, y1), (x2, y2),......, (x t , y t ), representing the observed points, and (x t+1 , y t+1 ) represents the new observed point.

[0134] Given the data D and the joint distribution expression of the new observed point (x t+1 , y t+1 ) is:

[0135]

[0136] where y 1：t+1 is the joint output of the observed data y1, y2,... y t and the new point y t+1 ; N represents the normal distribution; K is the Gram matrix, representing the covariance between the training data points; is the noise variance; I is the identity matrix; k represents the covariance between the new point x t+1 and the existing data points; k(x t+1 , x t+1 ) represents the autocovariance of the new point x t+1 .

[0137] At this time, predictions are made by considering the original observed data and the new observed point x t+1 . The expectation μ t+1 (x t+1 ) and variance σ t of y t+1 can also be obtained according to the posterior distribution of the new observed point y t+1 . The formulas are as follows:

[0138]

[0139] where μ t (x t+1 ) and σ t+1 are the expectation and variance of y t+1 respectively; y 1：t is the joint output of the observed data y1, y2,... y t ; is the noise variance; I is the identity matrix; k represents the covariance between the new point x t+1 and the existing data points; k(x t+1 , xt+1 ) represents the new point x t+1 's autocovariance. K is the Gram matrix, representing the covariance between training data points.

[0140] The ability of the Gaussian process to represent rich distributions depends only on the covariance function. The automatic relevance determination squared exponential function is used as the covariance function, and the formula is as follows:

[0141]

[0142] where K SE (x, x′) is the covariance between the input vectors x and x′. a0 is the overall variance parameter, controlling the overall scaling amplitude of the covariance function; b is the squared Euclidean distance for scaling the length scale of each dimension, x d is the d-dimensional component of the input vector x, x d ′ is the component of the input x′ in d; a d is the length scale parameter for the d-th dimension.

[0143] S303. Use the expected improvement covariance function α EI (x) as the acquisition function. The EI function takes into account the amount by which the unknown point improves over the known maximum value, and the formula is as follows:

[0144]

[0145] where μ(x) is the predicted mean of the Gaussian process) at point x; f(x + ) is the current known optimal objective function value; Φ(z) is the cumulative distribution function of the standard normal distribution; σ(x) is the predicted standard deviation of the Gaussian process at point x, and z is the normalization of the value at the current optimal point.

[0146] S304. Use the pre-collected non-slip dataset and slip dataset, and perform Bayesian optimization with 100 iterations respectively to obtain the optimal process noise and observation noise matrix parameters when there is no slip as:

[0147]

[0148] The optimal process noise and observation noise matrix parameters after slipping are:

[0149]

[0150] S4. After the slip is triggered, switch the optimal process noise matrix Q and observation noise matrix R during the pre-training process, update the above filtering parameters, and then output the high-precision arc length position and heading angle. Specifically, after determining the optimal process noise matrix (Q) and observation noise matrix (R) parameters in the slip and non-slip states, dynamically adjust these parameters according to the detected slip condition. In the case where no slip is detected, continue to use the optimized Q and R matrix parameters in the non-slip state. When more than two consecutive slip signals are detected, switch to the pre-trained optimal process noise matrix Q and observation noise matrix R applicable to the slip state. After updating these filtering parameters, output the high-precision distance that the robot has moved relative to the initial position, and the specific form of the output is the arc length relative to the initial position.

[0151] Embodiment 1

[0152] This embodiment analyzes the performance of the solution in the above embodiment, specifically as follows:

[0153] An encoder wheel and an imu are installed on the slider, the synchronous data reception frequency is 100 hz, the angular accuracy of the encoder wheel is 0.01°, the heading angle accuracy of the imu is 0.5°, the radius of the arc-shaped guide rail is 750 mm, and the moving distance (moving arc length) is read by a dividing meter with an accuracy of 0.01 mm. The moving distance curves obtained by using only the encoder wheel, only the imu, and the positioning algorithm of this embodiment are as Figure 2 shown. At the same time, the errors between the moving results obtained by using only the encoder wheel, only the imu, and the positioning algorithm of the present invention and the actual moving distance at the end of the movement are as Figure 3 shown. In the case of using the method of this embodiment, the error between the predicted moving distance and the true moving distance is smaller.

[0154] According to Figure 2 the trajectory to analyze the final error, the error when there is no slip is as Figure 3 shown, and the error when there is a slip is as Figure 4 shown. The present invention can achieve a high-precision positioning error within ±3 mm in the case of no slip, ensuring the precise positioning ability of the robot in a complex environment. The detection accuracy of the present invention for slip events exceeds 95%, significantly improving the reliability of the system, and can timely adjust the positioning strategy when a slip occurs, ensuring the continuity and accuracy of navigation and positioning. The method of this embodiment has a smaller error than using only the imu or the encoder wheel alone, indicating that the performance of the fusion positioning method is better than that of single-sensor positioning, and can still maintain a high accuracy in the slip state. The positioning accuracy of the underwater robot pipeline positioning algorithm is higher than that of the imu positioning, and the stability is higher than that of the encoder wheel which is prone to slip. It shows that the underwater robot pipeline positioning method has higher positioning accuracy and stability.

[0155] This embodiment adopts an underwater robot pipeline positioning method, uses Bayesian optimization to obtain optimal Kalman filter parameters, fuses the observation information of the encoder wheel and the imu, and performs Kalman filtering. Compared with the traditional single-sensor positioning method, the present invention can effectively improve the positioning accuracy and has stability under abnormal conditions such as wheel slip.

[0156] Each embodiment in this specification is described in a related manner. For the same or similar parts among the embodiments, reference can be made to each other. Each embodiment focuses on the differences from other embodiments. In particular, for the system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and for the related parts, reference can be made to the partial description of the method embodiment.

[0157] The above description is only the preferred embodiment of the present invention and is not intended to limit the protection scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention are all included in the protection scope of the present invention.

Claims

1. An underwater pipeline robot positioning method, characterized in that, Follow these steps: S1. A Kalman filter state model is established based on the underwater pipeline robot heading angle data, angular velocity data, encoder wheel rotation angle data, angular velocity data, and encoder wheel rotation angle data, angular velocity data publishing node and receiving node data. At the same time, the pipeline robot moving arc length change is calculated according to the heading angle change collected by the inertial measurement unit and combined with the pipeline radius, and the pipeline robot moving arc length change is used as an observation value to perform state update. S2, synchronized inertial measurement unit and encoder wheel data, using the dynamic relationship analysis between the linear velocity difference and the observed distance change to identify whether the underwater pipeline robot is slipping, and using the arc length change of the encoder and synchronized inertial measurement unit to compensate; S3, training the noise matrix Q and observation noise matrix R parameters of the underwater pipeline robot during the slipping and non-slipping process through offline Bayesian optimization; S4. After the slip is triggered, the optimal process noise matrix Q and observation noise matrix R in the pre-training process are switched, and the high-precision arc length position and heading angle are output after the process noise matrix Q and observation noise matrix R are updated to complete the positioning of the underwater pipeline robot.

2. The underwater pipeline robot positioning method according to claim 1, characterized in that The process of establishing the Kalman filter state model in S1 is specifically as follows: S101. Determine the arc length of the underwater pipeline robot moving on the pipeline: L e = θ e · πr / 180° L i = α i · πR p / 180° Among them, L e is the encoder observed arc length, L i is the inertial measurement unit observed arc length, θ e is the cumulative rotation angle of the encoder wheel, α i is the heading angle information directly obtained by the inertial measurement unit, r is the radius of the encoder wheel, R p is the pipe radius; S102, establish a Kalman filter state model: where, X k is the state vector at time k, A is the state transformation matrix, X k-1 is the state vector at time k-1, W k , V k are independent white noise matrices that follow a normal distribution, N represents the normal distribution, Z k is the observation vector at time k, is the process excitation noise covariance matrix, and R is the observation noise covariance matrix; Among them, the state vector is: X = [s v] T Where s is the arc length of the underwater pipeline robot moving on the pipeline, and v is the linear velocity of the underwater pipeline robot; The state transformation matrix A is: Where Δt is the sensor data receiving frequency; The observation matrix H is: S103: Estimate and correct the Kalman filter state model.

3. The underwater pipeline robot positioning method according to claim 2, wherein The estimation in S103 is to establish a priori estimation of the current state by using the time update equation, and timely forward calculate the value of the current state variable and the error covariance estimate to construct a priori estimation value for the next time state; The correction is to establish an improved a posteriori estimate of the current state based on the a priori estimate of the prediction process and the current measurement variable using the measurement update equation; Among them, the prior error E k - and the posterior error E k are respectively: The posterior error covariance matrix P k - and the prior error covariance matrix P k are respectively: P k - = E(E k - E k -T ) P k = E(E k E k T ) The time update equation is: P k - = AP k-1 A T + Q The state update equation is: K k = P k - H T (HP k - H T + R) -1 P k = (I - K k H)P k - Among them, is the state estimation at time k; is the state estimation at time k-1, is the state prior estimation at time k; A is the state transformation matrix; Q is the process excitation noise covariance matrix; I is the identity matrix; K k is the Kalman gain, R is the observation noise covariance matrix; H is the observation matrix; T is the transpose symbol; E is the expected variance matrix.

4. A method for positioning an underwater pipeline robot according to claim 1, characterized in that, The S2 is specifically performed according to the following steps: S201, using the message_filters package in the ROS system to synchronize the receiving timestamps of the encoder wheel and the data receiving end of the inertial measurement unit; After synchronizing the timestamps, obtain the moving distance provided by the encoder and the change in moving distance ΔL provided by the inertial measurement unit e and ΔL i is as follows: ΔL e = Δθ e · πr / 180° ΔL i = Δα i · πR p / 180° Among them, Δθ e is the difference in the cumulative rotation angle of the encoder between the current moment and the previous moment; Δα i is the difference in the heading angle recorded by the inertial measurement unit between the current moment and the previous moment, r is the radius of the encoder wheel, and R p is the radius of the pipeline; S203, determining the linear velocity provided by the encoder wheel and the linear velocity provided by the inertial measurement unit: V e = ω e · r V i = V z · R p Among them, V e is the linear velocity provided to the encoder wheel, ω e is the angular velocity of rotation of the encoder, r is the radius of the encoder wheel, V i is the linear velocity provided to the inertial measurement unit, V z is the angular velocity of rotation about the z-axis recorded by the inertial measurement unit, R p is the radius of the pipeline; S204, detecting whether the underwater pipeline robot is slipping based on the difference in speed and distance change; S205: When slippage is detected, the estimated value of the robot state is adjusted by cumulative error compensation method.

5. A method for positioning an underwater pipeline robot according to claim 1, characterized in that, The specific process of S204 is as follows: Use a sliding window to process the data sequences of the inertial measurement unit and the encoder wheel. Within the sliding window, if the difference in the distance change amounts recorded by the encoder wheel and the inertial measurement unit is too large, and there is also a large difference in the linear velocity, then the encoder wheel is in a slipping state. The specific condition is: ΔL e -ΔL i >3·σ Δs +0.1 and |V e -V i |>3·σ v ; where σ Δs and σ v are the standard deviations within the time window of the distance change amount and the velocity change amount respectively, V e is the linear velocity provided by the encoder wheel, and ΔL e and ΔL i are the moving distance change amounts provided by the encoder wheel and the inertial measurement unit at the slipping moment respectively.

6. The underwater pipeline robot positioning method according to claim 4, wherein The estimated value of adjusting the robot state by the cumulative error compensation method in S205 is specifically: L = L e -ΔE ΔE = ∑(ΔL e - ΔL i ) Among them, L is the observed arc length of the encoder wheel after compensation, and L e is the observed arc length of the encoder wheel at the slipping moment, ΔE is the cumulative slipping error, and ΔL e , ΔL i are the changes in the moving distance provided by the encoder wheel and the inertial measurement unit at the slipping moment, respectively; if there is no slipping, then L e is not processed, and E represents the expected variance matrix.

7. A method for positioning an underwater pipeline robot according to claim 1, characterized in that The specific process of S3 is as follows: S301. After completing the construction of the Kalman filter state model and realizing the encoder wheel slip detection and compensation, the process noise and observation noise when there is no slip and when there is slip are obtained by using the offline Bayesian optimization based on the Gaussian process, so as to minimize the difference between the data fusion result and the actual moving distance. The objective function is: x * = arg min f(x) where x * is a d-dimensional hyperparameter that optimizes the generalization performance of the model within the hyperparameter space, and f(x) is the mapping from the hyperparameter vector x to the model's generalization performance; S302. Use the Gaussian process function to fit f(x); S303. Use the expected improvement covariance function α EI (x) as the acquisition function; S304. Perform Bayesian optimization with 100 iterations on the non-slip dataset and the slip dataset of the underwater pipeline robot collected respectively, and obtain the optimal process noise and observation noise matrix parameters when there is no slip as follows: The optimal process noise and observation noise matrix parameters after slipping are as follows:

8. A method for positioning an underwater pipeline robot according to claim 1, characterized in that, The specific process of S4 is as follows: Dynamically adjust the optimal process noise matrix and observation noise matrix parameters in the slip and non-slip states based on the detected slip state of the underwater pipeline robot; in the case of no detected slip, continue to use the optimized process noise matrix and observation noise matrix parameters in the non-slip state. When more than two consecutive slip signals are detected, switch to the optimal process noise matrix and observation noise matrix parameters after slipping obtained by training in S3, and output the high-precision distance of the underwater pipeline robot moving relative to the initial position.

Images