# Robot Localization in Water Pipes Using Acoustic Signals and Pose Graph Optimization

^{1}

^{2}

^{3}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Methods: Acoustic Signal for Robot Localisation

#### 2.1. One-Dimensional Acoustic Signal

#### 2.2. Experimental Data

#### 2.3. Synthetic Data

## 3. Methods: Robot Localisation Using Pose Graph Optimization

#### 3.1. Conventional Pose Graph Optimization for Pipe Robots

#### 3.2. Pose Graph Optimization Using an Acoustic Signal

#### 3.3. Spatial Signal Information Methods

#### 3.3.1. Two-Point Linear Fit Prediction

#### 3.3.2. Quadratic Fit Prediction

#### 3.3.3. Cross-Correlation Matching

#### 3.3.4. Kernel Cross-Correlation Matching

#### 3.3.5. Phase-Correlation Matching

#### 3.4. Optimization Solution Methods

`lsqnonlin`function and Matlab’s Navigation Toolbox in the

`optimizePoseGraph`function.

## 4. Results

#### 4.1. Comparison of Methods

#### 4.2. Sensitivity to Measurement Noise

## 5. Discussion

#### 5.1. Summary of Results

#### 5.2. Comparison of Results

#### 5.3. Challenges and Implications

#### 5.3.1. Experimental Challenges

#### 5.3.2. Challenges in Constructing the Optimization Problem

#### 5.3.3. Challenges from Additional Dynamics

#### 5.3.4. Challenges from Sensitivity to Parameters

#### 5.4. Integration with Additional Information

#### 5.4.1. Integration with Additional Sensors

#### 5.4.2. Adaptation to Two or Three Dimensions

## 6. Conclusions

## Author Contributions

## Funding

## Acknowledgments

## Conflicts of Interest

## References

- GOV.UK. Water and Treated Water. 2015. Available online: https://www.gov.uk/government/publications/water-and-treated-water/water-and-treated-water (accessed on 17 July 2020).
- Government Office for Science. UK Water Research and Innovation Framework: Taking Responsibility for Water. 2012. Available online: https://assets.publishing.service.gov.uk/government/uploads/system/uploads/attachment_data/file/294176/11-1416-taking-responsibility-for-water-summary.pdf (accessed on 18 August 2020).
- The Consumer Council for Water. Water, Water Everywhere? Delivering Resilient Water and Waste Water Services Everywhere (2017–2018). 2017. Available online: https://www.ccwater.org.uk/research/water-water-everywhere-delivering-resilient-water-and-wastewater-services-2017-18/ (accessed on 20 July 2020).
- Durrant-Whyte, H.; Bailey, T. Simultaneous localization and mapping: Part I. IEEE Robot. Autom. Mag.
**2006**, 13, 99–108. [Google Scholar] [CrossRef] [Green Version] - Cadena, C.; Carlone, L.; Carrillo, H.; Latif, Y.; Scaramuzza, D.; Neira, J.; Reid, I.; Leonard, J.J. Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age. IEEE Trans. Robot.
**2016**, 32, 1309–1332. [Google Scholar] [CrossRef] [Green Version] - Krys, D.; Najjaran, H. INS Assisted Monocular Robot Localization. In Proceedings of the International Mechanical Engineering Congress & Exposition, Vancouver, BC, Canada, 12–18 November 2010. [Google Scholar] [CrossRef]
- Hansen, P.; Alismail, H.; Rander, P.; Browning, B. Visual mapping for natural gas pipe inspection. Int. J. Robot. Res.
**2015**, 34, 532–538. [Google Scholar] [CrossRef] - Zhao, W.; Kamezaki, M.; Yoshida, K.; Konno, M.; Onuki, A.; Sugano, S. Modeling and simulation of FLC-based navigation algorithm for small gas pipeline inspection robot. In Proceedings of the 2018 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM), Auckland, New Zealand, 9–12 July 2018; pp. 912–917. [Google Scholar] [CrossRef]
- Summan, R.; Jackson, W.; Dobie, G.; Macleod, C.; Mineo, C.; West, G.; Offin, D.; Bolton, G.; Marshall, S.; Lille, A. A novel visual pipework inspection system. AIP Conf. Proc.
**2018**, 1949. [Google Scholar] [CrossRef] [Green Version] - Kakogawa, A.; Komurasaki, Y.; Ma, S. Anisotropic shadow-based operation assistant for a pipeline-inspection robot using a single illuminator and camera. In Proceedings of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, 14 December 2017; pp. 1305–1310. [Google Scholar] [CrossRef]
- Thielemann, J.T.; Breivik, G.M.; Berge, A. Pipeline landmark detection for autonomous robot navigation using time-of-flight imagery. In Proceedings of the 2008 IEEE Computer Society Conference on Computer Vision and Pattern Recognition Workshops, CVPR Workshops, Anchorage, AK, USA, 15 July 2008. [Google Scholar] [CrossRef] [Green Version]
- Kakogawa, A.; Yamagami, T.; Tian, Y.; Ma, S. Recognition of pathway directions based on nonlinear least squares method. In Proceedings of the 2015 IEEE International Conference on Robotics and Biomimetics, IEEE-ROBIO 2015, Zhuhai, China, 6–9 December 2015; pp. 1596–1601. [Google Scholar] [CrossRef]
- Lee, D.H.; Moon, H.; Koo, J.C.; Choi, H.R. Map building method for urban gas pipelines based on landmark detection. Int. J. Control. Autom. Syst.
**2013**, 11, 127–135. [Google Scholar] [CrossRef] - Park, B.; Myung, H. Resilient Underground Localization Using Magnetic Field Anomalies for Drilling Environment. IEEE Trans. Ind. Electron.
**2018**, 65, 1377–1387. [Google Scholar] [CrossRef] - Guan, L.; Cong, X.; Sun, Y.; Gao, Y.; Iqbal, U.; Noureldin, A. Enhanced MEMS SINS Aided Pipeline Surveying System by Pipeline Junction Detection in Small Diameter Pipeline. IFAC-PapersOnLine
**2017**, 50, 3560–3565. [Google Scholar] [CrossRef] - Lim, H.; Choi, J.Y.; Kwon, Y.S.; Jung, E.j.; Yi, B.j. SLAM in Indoor Pipelines with 15mm Diameter. In Proceedings of the 2008 IEEE International Conference on Robotics and Automation, Pasadena, CA, USA, 13 June 2008; pp. 4005–4011. [Google Scholar] [CrossRef]
- Murtra, A.C.; Mirats Tur, J.M. IMU and cable encoder data fusion for in-pipe mobile robot localization. In Proceedings of the IEEE Conference on Technologies for Practical Robot Applications, TePRA, Woburn, MA, USA, 15 July 2013. [Google Scholar] [CrossRef] [Green Version]
- Santana, D.D.S.; Maruyama, N.; Furukawa, C.M. Estimation of trajectories of pipeline PIGs using inertial measurements and non linear sensor fusion. In Proceedings of the 2010 9th IEEE/IAS International Conference on Industry Applications, INDUSCON 2010, Sao Paulo, Brazil, 28 March 2011. [Google Scholar] [CrossRef]
- Chowdhury, M.S.; Abdel-Hafez, M.F. Pipeline Inspection Gauge Position Estimation Using Inertial Measurement Unit, Odometer, and a Set of Reference Stations. J. Risk Uncertain. Eng. Syst. Part B Mech. Eng.
**2016**, 2, 021001. [Google Scholar] [CrossRef] - Al-Masri, W.M.; Abdel-Hafez, M.F.; Jaradat, M.A. Inertial Navigation System of Pipeline Inspection Gauge. IEEE Trans. Control. Syst. Technol.
**2018**, 8, 609–616. [Google Scholar] [CrossRef] - Sahli, H.; El-Sheimy, N. A novel method to enhance pipeline trajectory determination using pipeline junctions. Sensors
**2016**, 16, 567. [Google Scholar] [CrossRef] - Guan, L.; Xu, X.; Gao, Y.; Liu, F.; Rong, H.; Wang, M.; Noureldin, A. Micro-Inertial-Aided High-Precision Positioning Method for Small-Diameter PIG Navigation. In Advances in Human and Machine Navigation Systems; IntechOpen: London, UK, 2018. [Google Scholar]
- Bando, Y.; Suhara, H.; Tanaka, M.; Kamegawa, T.; Itoyama, K.; Yoshii, K.; Matsuno, F.; Okuno, H.G. Sound-based online localization for an in-pipe snake robot. In Proceedings of the 2016 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), Lausanne, Switzerland, 15 December 2016; pp. 207–213. [Google Scholar] [CrossRef]
- Rizzo, C.; Kumar, V.; Lera, F. RF Odometry for Localization in Pipes Based on Periodic Signal Fadings. In Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, Chicago, IL, USA, 6 November 2014; pp. 4577–4583. [Google Scholar] [CrossRef]
- Ma, K.; Schirru, M.M.; Zahraee, A.H.; Dwyer-Joyce, R.; Boxall, J.; Dodd, T.J.; Collins, R.; Anderson, S.R. Robot mapping and localisation in metal water pipes using hydrophone induced vibration and map alignment by dynamic time warping. In Proceedings of the 2017 IEEE International Conference on Robotics and Automation (ICRA), Singapore, 24 July 2017; pp. 2548–2553. [Google Scholar] [CrossRef] [Green Version]
- Ma, K.; Schirru, M.; Zahraee, A.H.; Dwyer-Joyce, R.; Boxall, J.; Dodd, T.J.; Collins, R.; Anderson, S.R. PipeSLAM: Simultaneous Localisation and Mapping in Feature Sparse Water Pipes using the Rao-Blackwellised Particle Filter. In Proceedings of the 2017 IEEE International Conference on Advanced Intelligent Mechatronics (AIM), Munich, Germany, 3–7 July 2017; pp. 1459–1464. [Google Scholar] [CrossRef]
- Lu, F.; Milios, E. Globally Consistent Range Scan Alignment for Environment Mapping. Auton. Robot.
**1997**, 4, 333–349. [Google Scholar] [CrossRef] - Thrun, S.; Montemerlo, M. The graph SLAM algorithm with applications to large-scale mapping of urban structures. Int. J. Robot. Res.
**2006**, 25, 403–429. [Google Scholar] [CrossRef] - Dellaert, F.; Kaess, M. Square root SAM: Simultaneous localization and mapping via square root information smoothing. Int. J. Robot. Res.
**2006**, 25, 1181–1203. [Google Scholar] [CrossRef] [Green Version] - Grisetti, G.; Kummerle, R.; Stachniss, C.; Burgard, W. A tutorial on graph-based SLAM. IEEE Intell. Transp. Syst. Mag.
**2010**, 2, 31–43. [Google Scholar] [CrossRef] - Olson, E.; Leonard, J.; Teller, S. Fast Iterative Optimization of Pose Graphs with Poor Initial Estimates. In Proceedings of the IEEE International Conference on Robotics and Automation 2006, Orlando, FL, USA, 15–19 May 2006; pp. 2262–2269. [Google Scholar]
- Wang, T.; Peng, F.; Chen, B. Autonomous Localization and Mapping Using a Single Mobile Device. arXiv
**2016**, arXiv:1612.05793. [Google Scholar] - Jung, J.; Oh, T.; Myung, H. Magnetic field constraints and sequence-based matching for indoor pose graph SLAM. Robot. Auton. Syst.
**2015**, 70, 92–105. [Google Scholar] [CrossRef] - Gao, C.; Harle, R. MSGD: Scalable back-end for indoor magnetic field-based GraphSLAM. In Proceedings of the 2017 IEEE International Conference on Robotics and Automation (ICRA), Singapore, 29 May–3 June 2017; pp. 3855–3862. [Google Scholar] [CrossRef] [Green Version]
- Lu, C.X.; Li, Y.; Zhao, P.; Chen, C.; Xie, L.; Wen, H.; Tan, R.; Trigoni, N. Simultaneous localization and mapping with power network electromagnetic field. In Proceedings of the Annual International Conference on Mobile Computing and Networking, MOBICOM, New Delhi, India, 29 October–2 November 2018; pp. 607–622. [Google Scholar] [CrossRef] [Green Version]
- Thrun, S.; Burgard, W.; Fox, D. Probabilistic Robotics; The MIT Press: Cambridge, MA, USA, 2006. [Google Scholar]
- Wang, C.; Zhang, L.; Xie, L.; Yuan, J. Kernel cross-correlator. arXiv
**2018**, arXiv:1709.05936. [Google Scholar] - Gill, P.E.; Murray, W.; Wright, M.H. Practical Optimization; The Bath Press: Bath, UK, 1981. [Google Scholar]
- Björck, Å. Numerical Methods for Least Squares Problems; SIAM: Philadelphia, PA, USA, 1996. [Google Scholar]
- Lee, J.; Lim, H.; Song, S.; Myung, H. Concept Design for Mole-Like Excavate Robot and Its Localization Method. In Proceedings of the 2019 7th International Conference on Robot Intelligence Technology and Applications, RiTA, Daejeon, Korea, 1–3 November 2019; pp. 56–60. [Google Scholar] [CrossRef]
- Seco, T.; Lazaro, M.T.; Rizzo, C.; Espelosin, J.; Villarroel, J.L. Graph-Based Robot Localization in Tunnels Using RF Fadings. In Proceedings of the Robot 2019: Fourth Iberian Robotics Conference, Porto, Portugal, 20–22 November 2019. [Google Scholar]
- Worley, R.; Yu, Y.; Anderson, S. Acoustic Echo-Localization for Pipe Inspection Robots. In Proceedings of the IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems, Karlsruhe, Germany, 14–16 September 2020; pp. 2–7. [Google Scholar]
- Fang, X.; Wang, C.; Nguyen, T.M.; Xie, L. Graph Optimization Approach to Range-Based Localization. IEEE Trans. Syst. Man Cybern. Syst.
**2020**. [Google Scholar] [CrossRef] [Green Version] - Zuo, Z.; Liu, L.; Zhang, L.; Fang, Y. Indoor positioning based on bluetooth low-energy beacons adopting graph optimization. Sensors
**2018**, 18, 3736. [Google Scholar] [CrossRef] [Green Version] - Kümmerle, R.; Grisetti, G.; Strasdat, H.; Konolige, K.; Burgard, W. G2o: A general framework for graph optimization. In Proceedings of the 2011 IEEE International Conference on Robotics and Automation, Shanghai, China, 9–13 May 2011; pp. 3607–3613. [Google Scholar] [CrossRef]
- Alejo, D.; Caballero, F.; Merino, L. A robust localization system for inspection robots in sewer networks. Sensors
**2019**, 19, 4946. [Google Scholar] [CrossRef] [Green Version] - El Najjar, M.E.; Bonnifait, P. A Road-Matching Method for Precise Vehicle Localization Using Belief Theory and Kalman Filtering. Auton. Robot.
**2005**, 19, 173–191. [Google Scholar] [CrossRef] - Fouque, C.; Bonnifait, P.; Bétaille, D.; Working, A. Enhancement of Global Vehicle Localization using Navigable Road Maps and Dead-Reckoning. In Proceedings of the 2008 IEEE/ION Position, Location and Navigation Symposium, Monterey, CA, USA, 5–8 May 2008; pp. 1286–1291. [Google Scholar]
- Ruchti, P.; Steder, B.; Ruhnke, M.; Burgard, W. Localization on OpenStreetMap Data using a 3D Laser Scanner. In Proceedings of the 2015 IEEE International Conference on Robotics and Automation (ICRA), Seattle, WA, USA, 26–30 May 2015; pp. 5260–5265. [Google Scholar] [CrossRef]
- Brubaker, M.A.; Geiger, A.; Urtasun, R. Map-Based Probabilistic Visual Self-Localization. IEEE Trans. Pattern Anal. Mach. Intell.
**2016**, 38, 652–665. [Google Scholar] [CrossRef] [PubMed]

**Figure 1.**An example water supply pipe map for a region in the UK. The range of pipe layouts can be seen: straight pipes, curved pipes, and pipes with sharp corners. The scale of the map is 2 km from the bottom to the top.

**Figure 2.**The experimental measurement of a spatially varying acoustic property along the length of a water pipe: (

**a**) a photograph of the experiment at the University of Sheffield; (

**b**) vibration time-response from points along a pipe, recorded using a hydrophone to excite the vibration and to record the response amplitude; (

**c**) frequency spectra for a specific band (5 kHz to 40 kHz) for each position along a pipe, with the amplitude within a window (in this case 24.5 kHz to 27.5 kHz, shown by the red lines) summed to give a spatially varying signal along the pipe; and (

**d**) the resulting spatially varying signal along the length of the pipe, where two sets of measurements are shown superimposed and it can be seen that the signal is largely the same in each case.

**Figure 3.**The creation of synthetic 1D data based on the experimental data: (

**a**) the spatially varying acoustic signal superimposed on the trajectory of the robot along a pipe, illustrating how the signal would vary in the semi-two-dimensional space; (

**b**) the motion of the robot along the pipe in each direction, where a fixed command velocity is given, the robot’s actual velocity drifts with additive noise, and the spatial signal can be seen to be warped due to this drift; and (

**c**) a larger set of spatial signal data, simulated based on the experimental data.

**Figure 4.**An illustrative block diagram outlining the process described in Section 3: the symbols on each arrow describe the variables output from the functions described by a block. Each symbol is defined within Section 3. A typical pose-graph optimization would optimize a pose graph constructed using conventional measurements. This work augments the pose graph with information from acoustic measurements. The iterative nature of the process is not illustrated; the whole process is repeated for each new estimated trajectory x output from the optimization.

**Figure 5.**Illustrations of methods of taking information from the measurements of the spatially varying acoustic property: the information is used in the pose-graph optimization to improve the robot trajectory estimate. (

**a**) The function of the linear fit method: the two sets of poses and measurements (black crosses) should be aligned. The measurement at the pose in red (at 2.260 m) is predicted to be at the value of the red circle on the line between the two neighbouring points (at 2.255 m and 2.276 m). (

**b**) The function of the quadratic fit method: the two sets of poses and measurements (black crosses) should be aligned. The measurement at the pose in red (at 4.062 m) is predicted to be at the value of the red circle on the quadratic fit of the nearby points (shown in blue). (

**c**) An illustration of the cross-correlation method: an example of the matching parameters used to find correspondences in the two sets of data split into the labelled sections. $\gamma $ is the cross-correlation coefficient between the sections, $\eta $ is the inverse of the sum of squared error between the sections, and ${\left((1+\gamma )\eta \right)}^{2}$ is the parameter used to determine matches between sections by comparison to a threshold ${\tau}_{\gamma \eta}$. (

**d**) An illustration of the phase-correlation method used to find the offset between the sections of the signals in two sets of measurements: the two misaligned sections (${\mathrm{s}}_{1}$ and ${\mathrm{s}}_{2}$) are shown in the first plot. The second plot shows the cross-power function r, where the position of the largest value should correspond to the position shift which best aligns the two sections. The aligned sections are shown in the third plot.

**Figure 6.**Results from using the developed methods to improve the trajectory estimate in a single 1D pipe: (

**a**) an example of the results of the one-dimensional pipe trajectory optimization, using typical pose-graph optimization, illustrated by the resulting spatial signal estimation. The non-optimized estimate (blue) shows large inconsistency and error. The optimized estimate (red) shows some consistency, as the two signals are within the boundaries of the pipe, and accuracy, as the signals coincide well with the true spatial signal (black). The variance in the error, a measure of the inconsistency of the estimates, is shown along with the magnitude of the error for each point along the pipe. The velocity estimate is also shown, showing the drifting velocity in black and the simple estimate of a fixed velocity in red. (

**b**) An example of the results of the one-dimensional pipe trajectory optimization illustrated by the resulting spatial signal estimation using the quadratic fit prediction and phase-correlation methods: The non-optimized estimate (blue) shows large inconsistency and error. The optimized estimate (red) shows consistency, as the two signals are aligned, and accuracy, as the signals coincide well with the true spatial signal (black). The variance in error, a measure of the inconsistency of the estimates, is shown along with the magnitude of the error for each point along the pipe. The velocity estimate is also shown, showing the estimated velocity in red closely matching the drifting velocity in black.

**Figure 7.**A comparison of methods used to improve the trajectory estimate in a single 1D pipe: in all cases, proportional measures of error are used, which are normalized by the length of the pipe, to more easily allow comparison to other results. (

**a**) A comparison of the benchmark pipe-feature method with the different developed spatial field information methods in the 5-m pipe for a motion noise magnitude of 0.06 of the command motion, using box plots to show the quartiles of results over 50 sets of random noise: the top graph shows the error in the estimation, and the bottom graph shows the inconsistency in the estimation. On the left is the unoptimized (None), benchmark (Features), and improved estimate (PC+Quad) results. On the right is the results found using the other methods presented in this paper. (

**b**,

**c**) The median accuracy for each of the methods for the 5- and 20-m pipes respectively, over a range of noise magnitudes: the variance of the normally distributed noise added to the motion at each time step is equal to the noise magnitude multiplied by the command motion.

**Figure 8.**Results showing the robustness of the developed methods to noise in the measurement of spatially varying acoustic property: (

**a**,

**b**) example recorded acoustic signals over 5 m and 20 m with additive normally distributed noise with a standard deviation of 10 percent of the signal amplitude. (

**c**,

**d**) An analysis of the sensitivity of the estimation method to noise in the acoustic signal: for varying measurement noise magnitude (the variance of the normally distributed noise added to the signal measurement), a measure of the mean error was measured for both 5 m (with 0.06 motion noise) (

**c**) and 20 m (with 0.02 motion noise) (

**d**) pipes. The median is shown for each method. Slightly different parameters are used for the phase-correlation method compared to the results in Figure 7, reducing the threshold to which matches are compared, which allows matches to be found in the presence of noise.

**Figure 9.**A simplified illustration of the cost functions considered in this work: in this example, the aim would be to find the estimate which minimizes the cost function. The quality of a cost function is measured by how close its minimum is to the minimum of the true problem, which would correspond to the correct estimate. However, this is not possible directly. The implied problem is to find a trajectory estimate which is consistent with respect to measurements. This is not done directly and, instead, a cost function is defined, creating the constructed problem using the functions defined in Section 3.3. Shown are (

**a**) A constructed problem which matches well with the implied problem and (

**b**) a bad constructed problem.

**Figure 10.**A simplified illustration of the dynamic cost functions considered in this work: the constructed problem is designed to be a good match to the implied problem, so that the minimum of the implied problem can be approximately found. However, the constructed cost function is dynamic and changes as the optimization progresses. Illustrated are (

**a**) the cost function diverging from the implied function and (

**b**) the cost function converging towards a good approximation to the implied function.

© 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Worley, R.; Ma, K.; Sailor, G.; Schirru, M.M.; Dwyer-Joyce, R.; Boxall, J.; Dodd, T.; Collins, R.; Anderson, S.
Robot Localization in Water Pipes Using Acoustic Signals and Pose Graph Optimization. *Sensors* **2020**, *20*, 5584.
https://doi.org/10.3390/s20195584

**AMA Style**

Worley R, Ma K, Sailor G, Schirru MM, Dwyer-Joyce R, Boxall J, Dodd T, Collins R, Anderson S.
Robot Localization in Water Pipes Using Acoustic Signals and Pose Graph Optimization. *Sensors*. 2020; 20(19):5584.
https://doi.org/10.3390/s20195584

**Chicago/Turabian Style**

Worley, Rob, Ke Ma, Gavin Sailor, Michele M. Schirru, Rob Dwyer-Joyce, Joby Boxall, Tony Dodd, Richard Collins, and Sean Anderson.
2020. "Robot Localization in Water Pipes Using Acoustic Signals and Pose Graph Optimization" *Sensors* 20, no. 19: 5584.
https://doi.org/10.3390/s20195584