Concealed pipe detection method and storage medium based on trajectory reconstruction

Abstract

The present application relates to the field of trajectory reconstruction technology, discloses a dark pipe detection method and storage medium based on trajectory reconstruction, constructs and trains a trajectory reconstruction model based on an adaptive extended Kalman filter and an adaptive gradient descent algorithm, and includes step S1, collecting observation data of a target object; step S2, in the method of the adaptive extended Kalman filter, performing noise reduction processing on the observation data, and dynamically adjusting the noise covariance matrix to adjust the trust of the observation data; step S3, using an adaptive gradient descent algorithm to estimate the posture of the target object to establish the running trajectory of the target object; dynamically adjusting the step size in the adaptive gradient descent algorithm, if the motion state of the target object changes, adjusting the algorithm through a suppression factor; step S4, outputting the three-dimensional trajectory of the target object to complete the model training. The present application can effectively suppress the influence of noise on the system state estimation and improve the reconstruction accuracy and stability of the pipeline trajectory.

Description

Technical Field

[0001] The present application relates to the technical field of trajectory reconstruction, and in particular to a concealed pipe detection method and storage medium based on trajectory reconstruction. Background Art

[0002] Salt-alkali land is a major problem in agricultural production. In order to control salt-alkali land, salt drainage systems are widely used in water and soil improvement of saline-alkali land. Salt drainage systems usually consist of a series of underground pipes that drain excess salt from the soil, thereby improving the soil environment. However, due to the special environment of saline-alkali land, salt drainage pipes are often affected by corrosion, sedimentation, blockage and other factors, resulting in reduced function or failure of the salt drainage system.

[0003] In traditional ground detection methods, the high scattering and low permeability of saline-alkali land often interfere with the propagation of sound waves and radar signals, resulting in attenuation of echo signals, which affects the detection accuracy and reliability of salt drainage pipelines. In addition, these physical detection methods are often limited in their performance in reconstructing the trajectory of salt drainage pipelines. Ground penetrating radars can usually only provide local and intermittent pipeline information, making it difficult to continuously and accurately estimate the three-dimensional trajectory of salt drainage pipelines. Summary of the invention

[0004] The technical problem to be solved by the present application is to overcome the deficiencies of the prior art and provide a blind pipe detection method and storage medium based on trajectory reconstruction, which can realize the accurate reconstruction of the three-dimensional trajectory of the salt drainage blind pipe.

[0005] To achieve the above-mentioned purpose, the first aspect of the present application provides a concealed pipe detection method based on trajectory reconstruction, constructing and training a trajectory reconstruction model based on an adaptive extended Kalman filter and an adaptive gradient descent algorithm, and constructing the model includes the following steps:

[0006] Step S1, collecting observation data of the target object as input of the adaptive extended Kalman filter;

[0007] Step S2, in the method of adaptive extended Kalman filtering, in response to the input of step S1, performing noise reduction processing on the observation data, and dynamically adjusting the noise covariance matrix to adjust the confidence of the observation data;

[0008] Step S3, using the observation data outputted from step S2 as input of an adaptive gradient descent algorithm, and using the adaptive gradient descent algorithm to perform posture estimation on the target object to establish a running trajectory of the target object;

[0009] In response to the input of step S2, dynamically adjusting the step size in the adaptive gradient descent algorithm to avoid premature termination of model training, and if the motion state of the target object changes, adjusting the adaptive gradient descent algorithm by a suppression factor;

[0010] Step S4, outputting the three-dimensional trajectory of the target object to complete the model training.

[0011] Furthermore, in step S1, it also includes collecting an inertial signal of the target object, wherein the inertial signal includes acceleration and angular velocity;

[0012] In step S2, in the method of adaptive extended Kalman filtering, the inertial signal is preprocessed, and the preprocessing at least includes a step of dynamic weighted threshold control, and the noise covariance matrix is ​​adaptively adjusted through the step of dynamic weighted threshold control.

[0013] Furthermore, in the step of dynamically weighted threshold control, the measurement noise covariance matrix is ​​adjusted by dynamically adjusting the discrimination threshold. , the dynamic adjustment process is expressed as:

[0014] ;

[0015] ;

[0016] Where i represents the index number of the historical moment, N represents the total number of historical moments, and k represents the kth moment. is the gate indicator at time k, is the threshold at time k, is the threshold at time k+1, is the root mean square error of the measurement residual at the current time k, is the root mean square error of the measurement residual at time ki, ξ is the weight coefficient, is the rate of change of acceleration, is the weighted sum of historical RMS errors, and is the weight coefficient, is the time attenuation coefficient, is the historical threshold, is the noise covariance matrix at time k, is the noise covariance matrix at time k+1, μ is the adjustment coefficient, is the square of the norm of the acceleration vector.

[0017] Furthermore, in the attitude estimation, quaternion is used to represent the attitude of the target object. In step S3, the adaptive descent algorithm is expressed as:

[0018] ;

[0019] in, is the direction change rate measured by the gyroscope, β is the step size, ζ is the suppression factor, is the estimated attitude change rate, is the error function The gradient of The rate at which a parameter changes.

[0020] Further, in step S3, the adaptive gradient descent algorithm at least includes a method for dynamically adjusting the step length according to external acceleration, angular velocity and oscillation influence, and the oscillation influence at least includes bellows oscillation, dark tube deformation and sudden change of the motion state of the target object;

[0021] The dynamic adjustment of the step size is expressed as:

[0022] ;

[0023] in, , , are the components of angular velocity in the x, y, and z directions, is the second norm of the error function, λ is the adjustment coefficient, β is the step size, and ΔT is the sampling period.

[0024] Further, in step S3, if the weighted RMS value Exceeding the set threshold , the adaptive gradient descent algorithm is adjusted by the inhibition factor ζ, and the inhibition factor ζ is expressed as:

[0025] ;

[0026] Among them, τ Δ is the noise parameter, is the weighted RMS value of the three-axis acceleration readings at time t, To set the threshold.

[0027] Furthermore, between step S3 and step S4, the method further includes:

[0028] Step S30, converting the coordinate system of the observation data output in step S3 into a reference coordinate system;

[0029] Step S31, performing gravity compensation on the gravity acceleration in the reference coordinate system to obtain the acceleration after gravity compensation;

[0030] In step S31, the acceleration after gravity compensation is expressed as:

[0031] ;

[0032] in, , , Respectively represent the acceleration components on the x, y, and z axes after gravity compensation, , , Represent the components of the measured acceleration on the x, y, and z axes, respectively. is the actual gravitational acceleration value, is the acceleration after gravity compensation;

[0033] Step S32, integrating the acceleration after gravity compensation in step S31 twice, obtaining the velocity through the first integration and obtaining the displacement through the second integration;

[0034] In step S32, the compensated acceleration is calculated in the time period [ , ] to obtain the velocity v, expressed as:

[0035] ;

[0036] in, and Respectively represent the start and end time of the movement, is the initial velocity of the target object;

[0037] The velocity v obtained in the time period [ , ] to estimate the position coordinate p of the target object in the three-dimensional space, expressed as:

[0038] ;

[0039] in, is the initial position coordinate of the target object;

[0040] Step S33, connecting the three-dimensional coordinate points of each time step in time sequence, and mapping the coordinate sequence to the three-dimensional space to generate a continuous trajectory curve.

[0041] Further, in step S30, the acceleration data is converted from the inertial measurement unit coordinate system through the transformation matrix Converted to the reference coordinate system, the transformation matrix Based on the quaternion derivation, the reference coordinate system is the northeast celestial coordinate system, which is as follows:

[0042] ;

[0043] ;

[0044] in, is the transformation matrix, , , , are the components of the quaternion, is the transformed acceleration in the reference coordinate system, is the acceleration in the IMU coordinate system.

[0045] Furthermore, in step S32, a step of determining the motion state of the target object is also included. If the integrated acceleration is less than a preset threshold, the target object is in motion; if the integrated acceleration is greater than or equal to the preset threshold, the target object is in a stationary state, which is expressed as:

[0046] ;

[0047] ;

[0048] in, norm is the integrated value of the three-axis acceleration, , , Represent the components of the integrated acceleration on the x, y, and z axes, respectively. th is the preset threshold.

[0049] To achieve the above-mentioned purpose, the second aspect of the present application provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the concealed pipe detection method based on trajectory reconstruction as described above.

[0050] After adopting the above technical solution, the present application has the following beneficial effects compared with the prior art:

[0051] In the present invention, by performing noise reduction processing and attitude estimation on the collected data based on the adaptive extended Kalman filter and the adaptive gradient descent algorithm, the accurate reconstruction of the three-dimensional trajectory of the salt drainage hidden pipe can be achieved. Among them, the adaptive extended Kalman filter combines multiple variables to establish a dynamic weighted threshold control, and dynamically adjusts the discrimination threshold to adaptively adjust the noise covariance matrix, effectively suppressing the influence of noise on the system state estimation, solving the problem of filter divergence in a high-noise environment, and improving the reconstruction accuracy and stability of the pipeline trajectory; adjusting the compensation amount of the gradient descent by the suppression factor to reduce the trust in the accelerometer data, in the process of attitude estimation, optimize the accuracy of the direction estimation, and thus improve the accuracy of the pipeline trajectory reconstruction.

[0052] The specific implementation methods of the present application are further described in detail below in conjunction with the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] The accompanying drawings are part of this application and are used to provide a further understanding of this application. The illustrative embodiments of this application and their descriptions are used to explain this application, but do not constitute an improper limitation on this application. Obviously, the drawings described below are only some embodiments. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0054] In the drawings of the specification:

[0055] Figure 1 It is an overall logic diagram of the detection method in this specific implementation mode;

[0056] Figure 2 is a schematic diagram of the logic expansion of step S3 in this specific implementation mode;

[0057] Figure 3 is a logical schematic diagram of step S1 and step S2 in this specific implementation mode;

[0058] Figure 4 is a logical expansion diagram of the preprocessing step in this specific implementation mode;

[0059] Figure 5 is a schematic diagram comparing accelerometer data before and after filtering in this specific implementation mode;

[0060] Figure 5 (a) is a schematic diagram showing the comparison of the X-axis acceleration data before and after filtering;

[0061] Figure 5 Middle (b) is a schematic diagram comparing the Y-axis acceleration data before and after filtering;

[0062] Figure 5 Middle (c) is a schematic diagram comparing the Z-axis acceleration data before and after filtering;

[0063] Figure 6 is a schematic diagram comparing gyroscope data before and after filtering in this specific implementation mode;

[0064] Figure 6 (a) is a schematic diagram showing the comparison of the X-axis angular velocity data before and after filtering;

[0065] Figure 6 Middle (b) is a schematic diagram comparing the Y-axis upward angular velocity data before and after filtering;

[0066] Figure 6 Middle (c) is a schematic diagram comparing the Z-axis upward angular velocity data before and after filtering;

[0067] Figure 7 is a schematic diagram of comparison of roll angle curves in attitude estimation results in this specific implementation manner;

[0068] Figure 8 This is a schematic diagram of the comparison of the pitch angle curve in the attitude estimation result in this specific implementation mode;

[0069] Fig. 9 This is a schematic diagram of the comparison of the yaw angle curve in the attitude estimation result in this specific implementation method;

[0070] Fig.10 RMSE comparison diagram of the root mean square error in this specific implementation mode;

[0071] Fig.11 is a logic diagram of step S30, step S31 and step S32 in this specific implementation mode;

[0072] Fig.12 is a logical schematic diagram of the posture estimation step in this specific implementation mode;

[0073] Fig.13 Schematic diagram of comparison of reconstruction trajectories of various algorithms in this specific implementation method. DETAILED DESCRIPTION

[0074] In order to make the purpose, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. The following embodiments are used to illustrate the present application but are not used to limit the scope of the present application.

[0075] Due to the special environment of saline-alkali land, salt drainage pipelines are often affected by corrosion, sedimentation, blockage and other factors, which may lead to reduced function or failure of the salt drainage system. Traditional methods for detecting salt drainage hidden pipes mainly rely on manual inspections, ground detection or traditional underground pipeline detection technology. Manual detection methods require professionals to conduct on-site inspections, which is labor-intensive and has low detection efficiency. Since salt drainage hidden pipes are often buried deep and widely distributed, manual detection often cannot cover all pipes, resulting in a high probability of missed detection or misdetection. In traditional ground detection methods, the high scattering and low permeability of saline-alkali land often interfere with the propagation of sound waves and radar signals, resulting in attenuation of echo signals, affecting the detection accuracy and reliability of hidden pipes. In addition, these physical detection methods are often limited in terms of trajectory reconstruction. Ground penetrating radars can usually only provide local and intermittent pipeline information, making it difficult to continuously and accurately estimate the three-dimensional trajectory of the pipeline, and acoustic wave detection technology also lacks sufficient resolution to effectively track the overall path of the pipeline. Therefore, although physical detection methods can provide certain support for the detection of underground pipelines, they usually do not have the ability to accurately reconstruct the pipeline trajectory.

[0076] With the rapid development of machine learning algorithms, attempts have been made to apply them to the problem of pipeline trajectory reconstruction. Machine learning was first applied to the fusion of multi-source sensor data, especially the multi-dimensional data collected by inertial measurement units. IMU, which means inertial measurement unit in Chinese, is an important device in pipeline trajectory reconstruction. However, since the data it collects is easily affected by noise and drift, it is difficult to obtain accurate trajectory estimation by relying solely on IMU data. By fusing IMU data with data from other sensors and optimizing data with the help of machine learning algorithms, the accuracy and stability of trajectory reconstruction can be improved. However, its performance is still limited when dealing with complex nonlinear systems or highly dynamic environments, especially when there is significant noise interference, and the accuracy of state estimation needs to be further improved.

[0077] The key work of pipeline trajectory reconstruction is mainly concentrated on filtering and attitude estimation. In order to cope with the nonlinearity and complexity of inertial signals in dark pipe environments, the system can effectively utilize the high-frequency motion information provided by the IMU through the prediction update mechanism and adaptability of the Kalman filter. However, when the noise distribution of the observation data is uneven or there are strong time-varying characteristics, the adaptive adjustment mechanism may not be able to fully cope with these complex noise environments, resulting in the filter being unable to effectively improve the estimation results. Gradient descent algorithms of machine learning are applied to attitude estimation tasks. This type of algorithm gradually adjusts the model parameters by minimizing the loss number so that the trajectory estimate gradually approaches the true trajectory. However, the loss function may have multiple local minima, and the gradient descent algorithm is prone to fall into the local optimal solution, thus failing to find the global optimal solution. In addition, the gradient descent algorithm is difficult to achieve adaptive adjustment of the step size when facing complex sensor data, which may cause the model to oscillate or not converge during the iteration process, affecting the computational efficiency.

[0078] Based on this, see Figure 1 and Figure 2 The present application provides a method for detecting concealed pipes based on trajectory reconstruction, constructing and training a trajectory reconstruction model based on an adaptive extended Kalman filter and an adaptive gradient descent algorithm, and constructing the model includes the following steps:

[0079] Step S1, collecting observation data of the target object as input of the adaptive extended Kalman filter;

[0080] Step S2, in the method of adaptive extended Kalman filtering, in response to the input of step S1, performing noise reduction processing on the observed data, and dynamically adjusting the noise covariance matrix to adjust the confidence of the observed data;

[0081] Step S3, using the observation data outputted from step S2 as input to an adaptive gradient descent algorithm, and using the adaptive gradient descent algorithm to estimate the posture of the target object, so as to establish a running trajectory of the target object;

[0082] In response to the input of step S2, dynamically adjusting the step size in the adaptive gradient descent algorithm to avoid premature termination of model training, and if the motion state of the target object changes, adjusting the adaptive gradient descent algorithm by a suppression factor;

[0083] Step S4, output the three-dimensional trajectory of the target object to complete the model training.

[0084] It should be noted that the execution subject of the detection method is a salt-draining hidden pipe detection device for saline-alkali land based on trajectory reconstruction, and the device can be an electronic device, a component in an electronic device, an integrated circuit, or a chip. The electronic device can be a mobile electronic device or a non-mobile electronic device. Exemplarily, the mobile electronic device can be a mobile phone, a tablet computer, a laptop computer, a PDA, a vehicle-mounted electronic device, a wearable device, etc., and the non-mobile electronic device can be a server and a personal computer, etc., which are not specifically limited in this application. The following takes the execution subject as an example of a server to describe the salt-draining hidden pipe detection method for saline-alkali land based on trajectory reconstruction in this embodiment.

[0085] It should be noted that, in this embodiment, the saline-alkali land salt-draining pipe robot is taken as the target object for illustration. This application represents the trajectory of the saline-alkali land salt-draining pipe by reconstructing the three-dimensional motion trajectory of the saline-alkali land salt-draining pipe robot in the salt-draining pipe. The saline-alkali land salt-draining pipe in this embodiment adopts a corrugated pipe, and both the inner and outer walls of the corrugated pipe are provided with corrugations.

[0086] Relevant experiments have shown that the acceleration signal in the salt-draining dark pipe exhibits relatively obvious high-frequency vibrations, and the amplitude fluctuates greatly, especially in the Z-axis direction. This shows that the particularity of the material and structure of the salt-draining dark pipe causes strong vibration noise interference. In contrast, the acceleration signal in the smooth pipe is relatively stable, with a small fluctuation amplitude, indicating that the noise interference in this environment is relatively low. The angular velocity signal in the salt-draining dark pipe environment shows large fluctuations in the X, Y, and Z axes, with frequent changes in angular velocity and more drastic fluctuations. In contrast, the angular velocity signal in the smooth pipe environment is relatively stable, and the fluctuation amplitude of each axis is small, indicating that the angular velocity signal in the smooth pipe environment is less affected by external interference.

[0087] See also Figure 3 In this embodiment, step S1 further includes collecting an inertial signal of the target object, where the inertial signal includes acceleration and angular velocity;

[0088] In step S2, in the method of adaptive extended Kalman filtering, the inertial signal is preprocessed, and the preprocessing at least includes a step of dynamic weighted threshold control, through which the noise covariance matrix is ​​adaptively adjusted.

[0089] Specifically, in step S1, this embodiment uses the MPU-6050 sensor module with high integration and stability to collect accelerometer and gyroscope signals, wherein the accelerometer is used to collect acceleration and the gyroscope is used to collect acceleration. This application sets the range of the accelerometer to ±2g and the range of the gyroscope to ±2000° / s. In addition, the power supply voltage range of the sensor module is 3.3v-5v, and the sampling frequency is set to 50HZ to ensure the continuity and accuracy of signal acquisition.

[0090] See also Figure 4 In the preprocessing step in step S2, the following steps are specifically included:

[0091] Initialization step, initialize the error covariance matrix P0, process noise covariance matrix Q and measurement noise covariance matrix R, specifically expressed as:

[0092] ;

[0093] in, is the identity matrix, is the error variance of the initial estimate.

[0094] The state vector is represented as:

[0095] ;

[0096] in Represents the initial value of the quaternion.

[0097] In the prediction step, the state prediction is expressed as:

[0098] ;

[0099] Where f represents the state transfer function, u k is the control input (gyroscope data), is the posterior state estimate at time k.

[0100] The prediction covariance matrix is ​​expressed as:

[0101] ;

[0102] Among them, the state transfer matrix F k is the Jacobian matrix of f, is the covariance matrix of the process noise.

[0103] The measurement step, calculating the predicted measurement value is expressed as:

[0104] ;

[0105] The state space is mapped to the observation space through the nonlinear observation model function h(x), and the observation value at the next moment can be predicted based on the information at time k. .

[0106] Compute the measurement Jacobian matrix:

[0107] ;

[0108] Among them, H k is the Jacobian matrix of the measurement function h for x, which is used to approximate the observation model h in Linear representation of .

[0109] Update step: Kalman gain calculation is expressed as:

[0110] ;

[0111] in, is the measurement noise covariance matrix.

[0112] The status update is represented as:

[0113] ;

[0114] The error covariance update is expressed as:

[0115] .

[0116] See also Figure 1 and Figure 3 In this embodiment, in the step of dynamic weighted threshold control, the measurement noise covariance matrix is ​​adjusted by dynamically adjusting the discrimination threshold The dynamic adjustment process includes: calculating the root mean square error of the measurement residual at the current moment; calculating the acceleration change rate at the current moment; calculating the weighted sum of the historical root mean square errors; then weighting and summing these three factors through the corresponding weight coefficients to obtain the gating index; dynamically adjusting the measurement noise covariance matrix R according to the gating index, if the gating index exceeds the historical threshold, the noise covariance matrix is ​​adjusted, if the gating index is less than or equal to the historical threshold, the noise covariance matrix is ​​not adjusted, that is, the noise covariance matrix at time k+1 is equal to the noise covariance matrix at time k. The entire dynamic adjustment process is expressed as:

[0117] ;

[0118] ;

[0119] Where i represents the index number of the historical moment, N represents the total number of historical moments, and k represents the kth moment. is the gate indicator at time k, is the threshold at time k, is the gate indicator at time k+1, is the root mean square error of the measurement residual at the current time k, is the root mean square error of the measurement residual at time ki, ξ is the weight coefficient, is the rate of change of acceleration, is the weighted sum of historical RMS errors, and is the weight coefficient, is the time attenuation coefficient, The historical threshold is a preset threshold used with the gating indicator The comparison is used to decide whether to adjust the measurement noise covariance matrix. is the noise covariance matrix at time k, is the noise covariance matrix at time k+1, μ is the adjustment coefficient, is the square of the norm of the acceleration vector.

[0120] It should be noted that in the extended Kalman filter, the measurement noise covariance matrix It is usually set to a fixed value. However, during the movement of the robot for draining salt from saline-alkali land, the noise presents random signal characteristics. If a fixed measurement noise covariance matrix is ​​used , which may lead to reduced filtering accuracy and increased attitude solution errors.

[0121] In the step of adaptively adjusting the measurement noise covariance matrix, in order to enhance the adaptability of the filter in a dynamic environment, this embodiment is provided with a step of dynamic weighted threshold control, by dynamically adjusting the discrimination threshold to adaptively adjust the measurement noise covariance matrix .

[0122] This embodiment also fully captures the dynamic change characteristics of system noise through the weighted sum of historical data, thereby effectively reducing the dependence on the root mean square error at a single moment and avoiding excessive response of the system due to sudden noise.

[0123] It is worth noting that the dynamic adjustment of the gating threshold ensures that the system can adaptively adjust the degree of trust in the observed data in a complex and changing noise environment. and historical thresholds Perform smooth fusion to avoid estimation divergence caused by instantaneous fluctuations.

[0124] See also Figure 5 and Figure 6,In this embodiment, in order to verify the suppression performance of the ,improved adaptive extended Kalman filter for data noise, a set of ,comparative experiments are designed to evaluate the improvement effect of the ,adaptive extended Kalman filter (DWTEKF) in noise processing compared with the ,traditional extended Kalman filter (EKF).

[0125] from Figure 5 It can be clearly seen that the raw data RAW (green curve) has large fluctuations and noise. In the initial stage of the movement, since the noise covariance matrix and state estimation of DWTEKF have not yet fully converged, the filter needs to go through multiple iterations and dynamic adjustments to reach a stable state, so the initial noise suppression effect is not significant. However, as the movement time goes on, the filter gradually corrects the state estimation and dynamically adjusts the noise covariance matrix through multiple iterations. After sufficient convergence, the filter can more accurately capture the dynamic changes of the system. The inertial signal processed by DWTEKF (blue curve) has greatly reduced noise compared to EKF (orange curve), showing excellent noise suppression ability and signal fidelity. Similarly, in Figure 6 In the above analysis, although EKF reduces some noise to a certain extent, DWTEKF shows more superior performance when processing fast-changing dynamic signals. In contrast, DWTEKF can better adapt to noise changes in a dynamically changing environment and significantly improve the filtering effect and system stability.

[0126] It should be noted that Figure 5 and Figure 6 This is the noise suppression effect diagram. Figure 5 shows the comparison of accelerometer data before and after filtering, Figure 6 A comparison of gyroscope data before and after filtering is shown. Figure 5 The horizontal axis is time, and the vertical axis is the acceleration value on the three axes of X, Y, and Z. Figure 6 The horizontal axis is time, and the vertical axis is the angular velocity values ​​along the X, Y, and Z axes.

[0127] See also Figure 1 and Fig.12 , the step of posture estimation of the saline-alkali land salt drainage concealed pipe robot in step S3 is described in detail below.

[0128] It is worth noting that the purpose of executing step S3 is to determine the posture angle information of the saline-alkali land salt-draining pipe robot in the salt-draining pipe.

[0129] It should be noted that in order to ensure the accuracy of posture estimation, the quaternion method is used in this embodiment to represent the posture of the salt-draining robot in three-dimensional space. By using the quaternion method, not only can the universal lock problem be avoided, but also an efficient and non-singular rotation representation and interpolation method can be provided, which can continuously and consistently represent spatial rotation, and is particularly suitable for dynamic posture tracking in continuous time. The quaternion consists of a real part and three imaginary parts, expressed as:

[0130] ;

[0131] in, is a real number, , and is the imaginary part, i, j, k represent the components of the rotation axis.

[0132] When using quaternions to solve the carrier attitude, the quaternion at the initial moment must first be determined, which represents the carrier's starting attitude. Subsequently, by continuously collecting the angular velocity data measured by the gyroscope during the motion process, the time derivative of the attitude can be effectively calculated. The quaternion attitude update can be calculated by the following formula:

[0133] ;

[0134] ;

[0135] in, The quaternary derivative of the reference coordinate system relative to the IMU coordinate system q, that is, the attitude change rate, P ω is the angular velocity value measured by IMU in its own coordinate system, represents normalization to ensure that it represents a valid rotation, and ⊗ represents the quaternion multiplication operation.

[0136] When the initial conditions are known, the formula is discretized into the form of time steps to update the posture:

[0137] ;

[0138] Then integrate the formula to get the posture after a period of time. Through the recursive calculation of each time step, the continuous posture of the salt drainage pipe robot in saline-alkali land can be obtained to complete the posture update. Its representation is as follows:

[0139] .

[0140] In this embodiment, quaternions are used to represent the attitude of the target object in attitude estimation (in this embodiment, , , , are the components of the quaternion), in step S3, the adaptive descent algorithm is expressed as:

[0141] ;

[0142] in, is the rate of change of direction measured by the gyroscope, β is the step size, and ζ is the suppression factor. is the estimated attitude change rate, is the error function The gradient of the error function The rate at which a parameter changes.

[0143] Specifically, the error function The gradient of can be expressed as:

[0144] ;

[0145] Among them, the accelerometer data is used to represent the error function of the quaternion matrix, and the function is defined as follows:

[0146] .

[0147] In order to find the extreme value of the target error function, it is necessary to calculate the Jacobian matrix of the error function. After taking the quaternion derivative of the error function, the corresponding Jacobian matrix can be obtained:

[0148] ;

[0149] Substitute the obtained formula and its corresponding Jacobian matrix to calculate the gradient value of the error function:

[0150] .

[0151] In step S3 posture estimation, it should be noted that the accuracy of direction estimation is positively correlated with the accuracy of trajectory acquisition. However, the traditional gradient descent method uses a fixed step size, which makes the algorithm easy to fall into the local optimal solution, especially in high-dimensional non-convex loss functions. Although the adaptive gradient descent algorithm can adjust the step size, the step size will decrease monotonically during model training and eventually approach zero, which may cause the training process to stagnate prematurely.

[0152] In this embodiment, in the posture estimation step of step S3, at least a step of dynamically adjusting the step length according to the external acceleration, angular velocity and oscillation influence is included, wherein the oscillation influence is at least represented by bellows oscillation, dark tube deformation and sudden change of the motion state of the target object, and the dynamic adjustment of the step length is represented as:

[0153] ;

[0154] in, , , are the components of angular velocity in the x, y, and z directions, is the second norm of the error function, λ is the adjustment coefficient, and ΔT is the sampling period.

[0155] It is worth noting that in order to avoid the problem of local optimal solution caused by a fixed step size, this embodiment dynamically adjusts the step size according to the external acceleration, angular velocity and oscillation influence to optimize the accuracy of direction estimation and improve the accuracy of trajectory reconstruction.

[0156] It should be noted that in attitude estimation, this embodiment adjusts the step size according to the oscillation effect, where the oscillation effect may come from other physical structure changes in addition to the bellows oscillation. For example, deformation of physical structures such as concave and offset of dark pipes, or sudden changes in motion state when the system undergoes a sudden change, may have a serious impact on sensor data. These factors not only increase the noise level, but may also cause a sharp increase in the error of attitude estimation.

[0157] In this embodiment, in step S3, if the weighted RMS value Exceeding the set threshold , the adaptive gradient descent algorithm is adjusted by the inhibition factor ζ, and the inhibition factor is expressed as:

[0158] ;

[0159] Among them, τ Δ is the noise parameter, is the weighted RMS value of the three-axis acceleration readings at time t, To set the threshold, if Exceed , the adaptive gradient descent algorithm is adjusted by the inhibition factor.

[0160] Specifically, during the adaptive adjustment process, the inherent characteristics of the bellows may introduce an oscillation effect in the attitude estimation, especially when the salt drainage pipe is misplaced or the terrain is undulating, the speed of the salt drainage pipe robot in the saline-alkali land may undergo a sudden change, which will cause errors in the data collected from the sensor. In this case, the trust in the accelerometer data should be reduced, and the impact of the error should be mitigated by adjusting the compensation amount of gradient descent.

[0161] Specifically, in step S3, the three axial readings of the accelerometer are used as measurement data to calculate the weighted root mean square value of the three-axis readings at time t:

[0162] ;

[0163] in, x,t , y,t and z,t Represents the accelerometer's measurements in the x, y, and z directions at the current time t. and yes The predicted values ​​in the x, y and z directions are obtained by calculating the state estimation vector through the measurement model h, and the Jacobian matrix F of the state transfer equation and the measurement equation is calculated. k , H k Solve, solve the equation as follows:

[0164] ;

[0165] Parameter w x , w y and w z , represents the dynamic weight of the accelerometer in the x, y, and z axis directions. The weight is adjusted according to the RMS value of each axis. The specific calculation formula of the dynamic weight is as follows:

[0166] ;

[0167] Among them, γ is the balance factor, which is used to balance the influence of variance and RMS. x (t), RMS y (t) and RMS z (t) is the calculated RMS value for each axis. , and is the time-varying noise variance of each axis, estimated recursively:

[0168] ;

[0169] in, is the smoothing factor.

[0170] when Exceeding the set threshold When the robot is judged to have changed its motion state due to the physical structure deformation of the salt drainage pipe or the change of the environment inside the pipe, the test is triggered. In this case, the inhibition factor is introduced to adjust the algorithm.

[0171] It is worth noting that this embodiment uses the weighted root mean square value To evaluate the variability of the accelerometer's measurements in the three axes, the accelerometer data is weighted through a dynamic weight allocation mechanism and time-varying noise variance estimation, thereby giving greater weight to high-quality signals in the axes and effectively reducing the trust in abnormal or disturbed data. Exceeding the set threshold When , the gradient descent algorithm is adjusted accordingly through the suppression factor to reduce the impact of bad signals on the optimization process.

[0172] See also Figure 7 , Figure 8 , Fig. 9 and Fig.10 , the introduction of the suppression factor significantly limits the interference of noise on attitude estimation, reduces the violent fluctuation of attitude angle, effectively smoothes the attitude estimation curve, and makes the system more stable under external interference. This improvement is particularly obvious in the Pitch and Roll curves. Especially under noisy conditions, the system with the suppression factor shows a smaller root mean square error (RMSE) than the system without the suppression factor, especially after 15 seconds, the RMSE curve gradually stabilizes. This shows that the suppression factor significantly reduces the negative impact of noise on the estimation accuracy of the system by adaptively adjusting the step size, so that the attitude estimation error can be maintained at a low level, thus reflecting the significant improvement of the system in anti-interference ability and stability.

[0173] See also Figure 1 and Fig.11 In this embodiment, between step S3 and step S4, the steps further include:

[0174] Step S30, converting the coordinate system of the observation data output in step S3 into a reference coordinate system;

[0175] Step S31, performing gravity compensation on the gravity acceleration in the reference coordinate system to obtain the acceleration after gravity compensation;

[0176] In step S31, the acceleration after gravity compensation is expressed as:

[0177] ;

[0178] in, is the actual gravitational acceleration value, is the acceleration after gravity compensation; , , Respectively represent the acceleration components on the x, y, and z axes after gravity compensation, , , Represent the components of the measured acceleration on the x, y, and z axes respectively.

[0179] Step S32, integrating the acceleration after gravity compensation in step S31 twice, obtaining the velocity through the first integration and obtaining the displacement through the second integration;

[0180] In step S32, the compensated acceleration is calculated in the time period [ , ] to obtain the velocity v, expressed as:

[0181] ;

[0182] in, and Respectively represent the start and end time of the movement, is the initial velocity of the target object;

[0183] The velocity v obtained in the time period [ , ] to estimate the position coordinates p of the target object in three-dimensional space, expressed as:

[0184] ;

[0185] in, is the initial position coordinate of the target object;

[0186] Step S33, connecting the three-dimensional coordinate points of each time step in time sequence, and mapping the coordinate sequence to the three-dimensional space to generate a continuous trajectory curve.

[0187] See also Fig.11 , step S30 is described in detail below.

[0188] It is worth noting that after completing the posture estimation of the salt-draining pipe robot in saline-alkali land, the coordinate transformation is required first. When the salt-draining pipe robot in saline-alkali land moves in the salt-draining pipe, it will inevitably rotate, and the coordinate system of the IMU will be different from the reference coordinate system of the real world, which makes the acceleration measured by the accelerometer inconsistent with the acceleration of the real movement. Therefore, all filtered accelerations need to be converted from the IMU coordinate system to the reference coordinate system.

[0189] Please refer to Fig.11 In this embodiment, in step S30, the acceleration data is converted from the inertial measurement unit coordinate system through the conversion matrix Convert to reference coordinate system, transformation matrix Based on quaternion derivation, the reference coordinate system used in this embodiment is the northeast celestial coordinate system, which is as follows:

[0190] ;

[0191] ;

[0192] in, It is a transformation matrix used to convert the acceleration in the inertial measurement unit (IMU) coordinate system to the northeast celestial (ENU) coordinate system. Its elements are composed of the squares and products of quaternions. These items correspond to different rotation angles and directions. , , , is the component of the quaternion, representing the attitude information of the inertial measurement unit (IMU), is the transformed acceleration in the reference coordinate system, is the acceleration in the IMU coordinate system.

[0193] In order to transform the filtered acceleration from the body coordinate system to the reference coordinate system, this embodiment derives a transformation matrix based on quaternions. The transformation matrix represents the direction of the salt-draining robot in the reference coordinate system, which is obtained by the adaptive gradient descent algorithm:

[0194] ;

[0195] By using this transformation matrix , the acceleration data in the IMU coordinate system can be converted to the reference coordinate system.

[0196] Please continue to see Fig.11 , step S31 is described in detail below.

[0197] It should be noted that due to the effect of the earth's gravity, the saline-alkali land salt drainage pipe robot is always affected by the earth's gravity during movement, and the accelerometer will be disturbed by the gravity acceleration, making it difficult to correctly measure the actual acceleration information of the saline-alkali land salt drainage pipe robot. If the uncorrected acceleration data is used in the trajectory reconstruction model, it will not be able to accurately reflect the actual dynamics of the saline-alkali land salt drainage pipe robot, resulting in inclination or deviation in the calculation of its three-dimensional motion trajectory.

[0198] Therefore, step S31 needs to be executed before step S32, and the gravity acceleration is compensated by step S31. In order to determine the actual gravity acceleration value in this embodiment, when the saline-alkali land salt drainage pipe robot is in a stationary state, multiple groups of acceleration values ​​should be measured and the average value should be calculated as the actual gravity acceleration value. The acceleration after gravity compensation can be calculated by the following formula:

[0199] ;

[0200] Among them, g s is the actual gravitational acceleration value, a g is the acceleration after gravity compensation.

[0201] Please continue to see Figure 1 and Fig.11 , step S32 is described in detail below.

[0202] After executing step S30 and step S31, the compensated acceleration needs to be integrated twice to obtain the displacement. Specifically, the velocity is obtained by the first integration in step S32, and the displacement is obtained by the second integration.

[0203] Specifically, the acceleration after compensation is in the time period [ , ] is integrated for the first time to obtain the speed v, and the obtained speed v is integrated twice within the time period [t1, t2] to estimate the position coordinates p of the saline-alkali land salt drainage pipe robot in the three-dimensional space.

[0204] Please continue to see Fig.11 In this embodiment, in step S32, a step of determining the motion state of the target object is also included. If the integrated acceleration is less than a preset threshold, the target object is in motion. If the integrated acceleration is greater than or equal to the preset threshold, the target object is in a stationary state, which is expressed as:

[0205] ;

[0206] ;

[0207] in, norm is the integrated value of the three-axis acceleration, , , Represent the components of the integrated acceleration on the x, y, and z axes, respectively. th is the preset threshold.

[0208] It is worth noting that for the integration processing of acceleration, threshold setting is the key. Noise and drift are inevitable in the inertial signal acquisition process of the IMU module. If a threshold is not set to determine whether the saline-alkali land salt drainage robot is in a stationary or moving state, these noises and drifts will gradually accumulate, resulting in significant errors in speed and position calculations. Over time, these errors may cause the trajectory to deviate from the actual path.

[0209] In this embodiment, the acceleration collected during the movement of the saline-alkali land drainage pipe robot is integrated, and a threshold is introduced to identify the movement state. norm Less than the set threshold th When the integrated acceleration norm Greater than or equal to the set threshold th When , the saline-alkali land salt drainage concealed pipe robot is regarded as being in motion.

[0210] Finally, execute step S33, connect the three-dimensional coordinate points of each time step in time sequence, and map the coordinate sequence to three-dimensional space to generate a continuous trajectory curve, thereby constructing a path diagram of the saline-alkali land salt-draining pipe robot during the entire movement process, and the trajectory of the salt-draining pipe is represented by the three-dimensional motion trajectory of the saline-alkali land salt-draining pipe robot.

[0211] See also Fig.13 In this embodiment, in order to verify the improvement of trajectory accuracy of the present application, the trajectory obtained by the experiment is compared with the preset trajectory. The results are as follows Fig.13 shown. Fig.13 Comparison diagram of the trajectories of each algorithm.

[0212] Specifically, in Fig.13 In the figure, the red curve represents the trajectory preset in the experiment, and the blue curve is the three-dimensional trajectory generated by the algorithm proposed in this embodiment. The orange curve and the green curve correspond to the trajectories obtained by the AGADA and GDA attitude estimation algorithms, respectively. In the initial stage of dark pipe reconstruction, the accuracy of the direction estimation of the three algorithms is not much different, and the degree of fit with the preset reference trajectory is high, showing good results. However, with the increase of the movement distance, due to the difference in algorithm performance, the trajectory error begins to accumulate, and the trajectories generated by the AGDA and GDA algorithms gradually deviate from the reference trajectory, among which the GDA deviation is the largest, showing a large error. In contrast, the trajectory generated by the adaptive gradient descent algorithm (AGDSD) of this embodiment always maintains a relatively high accuracy throughout the movement process, the trajectory deviation is the smallest, and the degree of fit with the true trajectory is the best. Through a comprehensive analysis of the results of the first three experiments, it can be clearly seen that the AGDSD algorithm of this embodiment has superior performance in complex environments, especially when dealing with complex pipeline structures such as depressions and offsets. The algorithm can effectively suppress error accumulation and achieve high-precision trajectory reconstruction.

[0213] Based on the same application concept, the present application also provides a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, the hidden pipe detection method based on trajectory reconstruction as described above is implemented.

[0214] The term "comprise" or any other similar term is intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus / device that includes a list of elements includes not only those elements but also other elements not expressly listed, or also includes elements inherent to such process, method, article, or apparatus / device.

[0215] The above are only preferred embodiments of the present application, and are not intended to limit the present application in any form. Although the present application has been disclosed as above with preferred embodiments, it is not intended to limit the present application. Any technician familiar with the present application can make some changes or modifications to equivalent embodiments of equivalent changes using the above-mentioned technical contents without departing from the scope of the technical solution of the present application. The implementation schemes in the above embodiments can also be further combined or replaced. However, any simple modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present application without departing from the content of the technical solution of the present application are still within the scope of the solution of the present application.

Claims

1. A hidden pipe detection method based on trajectory reconstruction, characterized in that: A trajectory reconstruction model based on an adaptive extended Kalman filter and an adaptive gradient descent algorithm is constructed and trained. The construction of the model includes the following steps: Step S1, collecting observation data of the target object as input of the adaptive extended Kalman filter; Step S2, in the method of adaptive extended Kalman filtering, in response to the input of step S1, performing noise reduction processing on the observation data, and dynamically adjusting the noise covariance matrix to adjust the confidence of the observation data; Step S3, using the observation data outputted from step S2 as input of an adaptive gradient descent algorithm, and using the adaptive gradient descent algorithm to perform posture estimation on the target object to establish a running trajectory of the target object; In response to the input of step S2, dynamically adjusting the step size in the adaptive gradient descent algorithm to avoid premature termination of model training, and if the motion state of the target object changes, adjusting the adaptive gradient descent algorithm by a suppression factor; Step S4, outputting the three-dimensional trajectory of the target object to complete the model training; In step S1, it also includes collecting an inertial signal of the target object, wherein the inertial signal includes acceleration and angular velocity; In step S2, in the method of adaptive extended Kalman filtering, the inertial signal is preprocessed, and the preprocessing at least includes a step of dynamic weighted threshold control, and the noise covariance matrix is ​​adjusted adaptively through the step of dynamic weighted threshold control; In the step of dynamic weighted threshold control, the measurement noise covariance matrix R is adjusted by dynamically adjusting the discrimination threshold. The dynamic adjustment process is expressed as: Where i represents the index number of the historical moment, N represents the total number of historical moments, k represents the kth moment, and Jk is the gated index at moment k. is the threshold at time k, is the threshold at time k+1, e RMS is the root mean square error of the measurement residual at the current time k, is the root mean square error of the measurement residual at time ki, ξ is the weight coefficient, Δa k is the rate of change of acceleration, is the weighted sum of the historical RMS errors, ε1 and ε2 are weight coefficients, ρ is the time decay coefficient, J th is the historical threshold, R k is the noise covariance matrix at time k, R k+1 is the noise covariance matrix at time k+1, μ is the adjustment coefficient, ||a k || 2 is the square of the norm of the acceleration vector.

2. The method for detecting concealed pipes based on trajectory reconstruction according to claim 1, characterized in that: In step S3, the attitude estimation uses quaternion to represent the attitude of the target object. In step S3, the adaptive descent algorithm is expressed as: in, is the direction change rate measured by the gyroscope, β is the step size, ζ is the suppression factor, is the estimated attitude change rate, is the error function The gradient of The rate at which a parameter changes.

3. The hidden pipe detection method based on trajectory reconstruction according to claim 2 is characterized in that: In step S3, the adaptive gradient descent algorithm at least includes a step of dynamically adjusting the step length according to external acceleration, angular velocity and oscillation influence, wherein the oscillation influence at least includes bellows oscillation, dark tube deformation and sudden change of the motion state of the target object; The dynamic adjustment of the step size is expressed as: Among them, ω Px ,ω Py ,ω Pz are the components of angular velocity in the x, y, and z directions, is the second norm of the error function, λ is the adjustment coefficient, β is the step size, and ΔT is the sampling period.

4. The hidden pipe detection method based on trajectory reconstruction according to claim 2 is characterized in that: In step S3, if the weighted RMS value δ RMS Exceeding the set threshold δ RMSth , the adaptive gradient descent algorithm is adjusted by the inhibition factor ζ, and the inhibition factor ζ is expressed as: Among them, τ Δ is the noise parameter, δ RMS is the weighted RMS value of the three-axis acceleration readings at time t, δ RMSth To set the threshold.

5. The method for detecting concealed pipes based on trajectory reconstruction according to claim 4, characterized in that: Between step S3 and step S4, the method further includes: Step S30, converting the coordinate system of the observation data output in step S3 into a reference coordinate system; Step S31, performing gravity compensation on the gravity acceleration in the reference coordinate system to obtain the acceleration after gravity compensation; In step S31, the acceleration after gravity compensation is expressed as: Among them, a gx 、a gy 、a gz Respectively represent the acceleration components on the x, y, and z axes after gravity compensation, a Ex 、a Ey 、a Ez Represents the measured acceleration components on the x, y, and z axes, g s is the actual gravitational acceleration value, a g is the acceleration after gravity compensation; Step S32, integrating the acceleration after gravity compensation in step S31 twice, obtaining the velocity through the first integration and obtaining the displacement through the second integration; In step S32, the compensated acceleration is integrated for the first time in the time period [t1, t2] to obtain the velocity v, which is expressed as: Wherein, t1 and t2 represent the start time and end time of the movement respectively, and v0 is the initial velocity of the target object; The obtained velocity v is integrated twice in the time period [t1, t2] to estimate the position coordinate p of the target object in the three-dimensional space, which is expressed as: Wherein, p0 is the initial position coordinate of the target object; Step S33, connecting the three-dimensional coordinate points of each time step in time sequence, and mapping the coordinate sequence to the three-dimensional space to generate a continuous trajectory curve.

6. The method for detecting concealed pipes based on trajectory reconstruction according to claim 5, characterized in that: In step S30, the acceleration data is converted from the inertial measurement unit coordinate system through the transformation matrix Converted to the reference coordinate system, the transformation matrix Based on the quaternion derivation, the reference coordinate system is the northeast celestial coordinate system, which is as follows: in, is the transformation matrix, q0, q1, q2, q3 are the components of the quaternion, a E is the transformation acceleration in the reference coordinate system, a P is the acceleration in the IMU coordinate system.

7. The method for detecting concealed pipes based on trajectory reconstruction according to claim 5, characterized in that: In step S32, the step of determining the motion state of the target object is also included. If the integrated acceleration is less than a preset threshold, the target object is in motion. If the integrated acceleration is greater than or equal to the preset threshold, the target object is in a stationary state, which is expressed as: Among them, a norm is the integrated value of the three-axis acceleration, a x 、a y 、a z Represents the components of the integrated acceleration on the x, y, and z axes, respectively. th is the preset threshold.

8. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the hidden pipe detection method based on trajectory reconstruction as described in any one of claims 1 to 7 is implemented.

Images