Combining Cameras, Magnetometers and Machine-Learning into a Close-Range Localization System for Docking and Homing Marc Hildebrandt and Leif Christensen and Frank Kirchner Underwater Robotics Department DFKI RIC Bremen Germany, 28359 Bremen Email:
[email protected]
Abstract—In this work we are describing a multi-modal shortrange navigation system for precision positioning tasks such as docking of a robotic vehicle in 3d space. The two input modalities, a monocular camera tracking a visual marker and an array of 3-axis magnetometers tracking a magnet, were chosen to cover a wide range of environmental conditions in order to increase the versatility and robustness of the navigation system. A comprehensive analysis of the individual components of the system is provided, resulting in a reliable estimate of its realworld applicability.
I. I NTRODUCTION A number of tasks require very high precision localization of a vehicle relative to a structure or other vehicles. Examples are docking (both into a station or with another vehicle), inspection or geo-referencing. While a number of different sensors have been used for this purpose in the past, we developed a bimodal system fusing magnetic data of 3axis-magnetometers and data extracted from a camera for our approach. Using fundamentally different modalities for data acquisition significantly increases fault-tolerance, since disturbances of one source of information is unlikely to affect the other. In this case poor visual conditions might hinder the camera, while external magnetic fields or metallic structures might interfere with the magnetic sensors. There are a number of different docking systems for AUVs (Autonomous Underwater Vehicles), which can be roughly categorized by the vehicle’s approach strategy: AUVs which rely on forward motion for depth-keeping (dynamic diving AUVs) usually feature a rather large passive guidance funnel, which serves as both guidance and breaking contraption (e.g. [1]). Docking stations for hovering-enabled AUVs have a higher degree of diversity, ranging from garage-type stations [2] [3], stations resembling a cradle [4] to miniature docking stations [5]. Typical approach strategies include acoustic homing [6], electromagnetic guidance [7] and visual approaches [8] [9]. Due to the high positioning accuracy of vision-based sensors they are the preferred method for the final docking approach. Magnetometers have been most prominently used for AUVs as orientation measurement devices, either in the form of simple compasses or more sophisticated multi-sensor approaches [10].
The paper is structured as follows: after this introduction, we describe our close range navigation approach in section II and show experimental results in section III. We then conclude our work and give an outlook in section IV. II. D ESCRIPTION The navigation system consists of the sensors mounted onto the robotic vehicle and the markers attached to a base-station. The sensors on the robotic vehicle are a monocular camera and a set of three 3-axis magnetometers. The base-station is equipped with a visual marker as well as a strong rare-earth magnet (cube with 1cm edges, N52 magnetization). The basic idea is to track the visual marker with the monocular camera as well as detect the magnetic field of the rare-earth magnet using the magnetometers. As will be shown, both methods provide an independent estimate of position of the sensors relative to the base-station. The basic setup is depicted in figure 1. The position estimates of both sensor modalities are fused, resulting in a single estimate of relative position which can be used for navigation. Extracting relative position data with a camera-marker setup is relatively simple and has been used numerously for similar purposes [11]. Given the utilization of robust markers such as April-Tags [12], Aruco-Markers [13] or similarly encoded 2-d visual markers a calibrated monocular camera can extract the marker’s 3d position and orientation, if the size of the marker is known. The limiting factors of this approach are image and processing rate on the one hand and precision with respect to field of view, distance and resolution of the camera on the other hand. Typical cameras run with approx. 30 Hz, which should be sufficient for most navigation purposes. Processing complexity is directly proportional to the camera’s resolution, but it has been shown that even with moderate processing hardware and typical cameras processing rates of 30 Hz are well achievable [11]. Precision is more complex, as it depends on three factors: the camera’s field of view, resolution of the camera and distance of the marker/marker size. For the case of close-range navigation of interest in our application sub-cm accuracy can be achieved.
Our machine-learning approach exploits the fact that we have a modality for relative position measurement. This enables us to collect magnetometer data in the work-space of the navigation while also recording position data from the cameras, once the turbidity in the water column allows us to do so (but one time is sufficient). This dataset can then be used to train a Support Vector Machine for function regression (Support Vector Regressor - SVR) or an artificial neural network (ANN), in our case a multi-level perceptron regressor (MLPR, for details see section III-B). After training, the SVR or MLP regressor can be used in real-time to estimate the position of the rare-earth magnet relative to the magnetometers. Limiting factors for this modality are distance between magnet and magnetometers, strength of the rare-earth magnet and dynamic range of the magnetometers. Since all three are dependent it is important to select the strength of the magnet and the dynamic range of the magnetometers according to the size of the workspace in order to achieve maximum precision. Also work space coverage and marker tracking accuracy will have a huge effect on the usability of the training data. Fig. 1: Basic setup of the navigation system: 1: base-station, 1.1: visual marker, 1.2: rare-earth magnet, 2: sensors on robotic vehicle, 2.1: monocular camera, 2.2: 3-axis magnetometers (3), 3.1: magnet detection radius, 3.2: visual marker detection area.
While marker-tracking and other vision-based approaches tend to have high accuracy in position estimation in proper visibility conditions, they lack robustness and are prone to errors in demanding environments, especially in turbid waters (e.g. river deltas, harbor basins, subsea production / mining fields, etc.). Another sensor modality often used on AUVs are therefore acoustic approaches to complement the optical methods. But acoustic systems usually don’t have the accuracy of the optical systems and also have to struggle with changing sound propagation characteristics due to varying salinity, conductivity and temperature in the water column. Opposed to that, the earth magnetic field is not disturbed by these effects. Although the pole’s location is changing over time and the magnetic field is significantly locally distorted depending on latitude and longitude of the observer, the declination from magnetic to true north can be computed using analytical models like the World Magnetic Model (WMM) [14]. Apart from this variation coming from the main magnetic field density contributors (the earth core with its geodynamo effect and the earth crust), the earth magnetic field can be considered sufficiently stable in the temporal domain for our purpose [15]. Local magnetic field diversions may occur due to the magnetic remanence of nearby material or strong electrical currents [16], but calibration or compensation approaches exist to counter these vehicle-immanent distortions [10], [15], [17]. The remaining distortions of the ambient magnetic field caused by the docking structure (even intensified by attaching strong neodym magnets) carry the information, we utilize in our method to estimate the relative position of our vehicle.
III. E XPERIMENTS All experiments were performed with the following sensor setup: a Allied Vision Prosilica GC 1380h digital camera with a 1280x1024 resolution using a 87 deg lens and four ST LSM303D 3-axis magnetometers in a circular arrangement around the camera lens. The base-station consisted of a 10x10cm Aruco-type marker and a neodymium magnet. The test system is shown in figure 2. Since the navigation system is to be used for docking purposes with an AUV (autonomous underwater vehicle), the sensors are water proofed (camera in a housing, magnetometers cast). Depending on the experiment they are either used as a separate, externally moved test-setup or inside the vehicle.
Fig. 2: Setup of the test-system consisting of the docking-station mounted on the z-axis of the gantry crane (1), the camera (2) and magnetometer sensors (3). The illumination was added for better camera performance. The hoses seen in the background are not part of this experiment.
A. Positioning Accuracy of Camera/Marker Tracking with Gantry Crane In order to estimate the accuracy of the camera/marker tracking an experiment was conducted where the base-station was fixed and the test-setup moved externally by a 3-axis gantry crane. The resulting dataset is used to characterize the optical tracking system (following the principles described in
[18]), since the marker tracking is used later to gather training data for the magnetometer-based localization. The gantry crane has a positioning accuracy of < 0.2mm [19], its position is handled as perfect ground-truth without uncertainty for the purpose of data processing. The work-space of the localization (0.4x0.4x0.2m) was discretized into a 0.05m grid, the resulting 256 positions are automatically traced by the gantry crane (see figurefig:trajectory). At each position both magnetometer data and camera data are recorded. The resulting accuracy of the z-axis is shown in figure 4. While the standard deviations for the x- and y-axis remain constantly at 0.0007, the standard deviation for the z-axis improves with a reduction of distance from 0.01233 at 0.35m to 0.0054 at 0.15m. This was to be expected, since the effective resolution for size estimation of the marker improves at close distances. The mean absolute error for the x- and y-axis is 0.003m, for the z-axis 0.007m at 0.35m distance and 0.002m at 0.15m distance. It has to be noted, that there were a number of drop-outs at positions where the marker was not fully visible in the camera image (as can be seen in figure 5). Due to the high accuracy of the marker tracking its uncertainty was not taken into account when training the machinelearning algorithms. The docking-adapter was designed for an accuracy of > 1cm, and the results presented in section III-A meet that requirement. The fact, that z-axis accuracy improves when the marker was closer to the camera fits a docking scenario in the sense, that higher precision is needed when the vehicle closes in to the docking station.
Fig. 4: Results of the camera/marker tracking experiment. The graph shows the gantry crane z-axis position relative to the camera over time as well as the extracted marker position.
(a) Gantry-X vs Marker-X
(b) Gantry-Y vs Marker-Y
Fig. 5: The Graph shows the position of the gantry crane in x and y direction and the extracted marker positions. Note the drop-outs at the edges of the image.
Fig. 3: Measurement points taken by the gantry crane.
B. Machine Learning Positions from Magnetic Field Data The data gathered as described in section III-A was processed using the scikit-learn machine learning framework [20]. In order to filter out high frequency noise, we first apply a second order Butterworth filter depending on the sampling Nyquist frequency (7). The magnetometer values as input for the models to be trained were standardized to have zero mean and unit variance.
The data set used for this work consisted of 442 samples with 12 magnetometer and 3 position features each. The magnetometers where sampling at 100 Hz and the marker tracker was estimating poses with 0.72 Hz. All magnetometer readings except those occurring when a marker pose was estimated where discarded. In order to prevent overfitting, we split the data using random permutation into a training data set (60%) and a test data set (40%). Before training the SVR and MLPR, we applied a grid search using the training data set to find the optimal meta parameters. The grid search suggested a C-value of 1.0 and an -value of 0.1 for the SVR. With the SVR approach, we have to train one support vector machine for each of the three dimensions of the target offset vector, which somewhat neglects the fact, that these 3 components are inherently coupled, because they describe a magnetic flux density vector, incorporating field orientation and strength. For demonstration purposes we also used a linear kernel with the SVR aside from the more capable radius basis function (RBF) kernel. Suggested parameters for the MLPR were an α-value of 0.01 and 3 hidden layers of size 40, 30 and 20 perceptrons. The grid search also favored LBFGS over Adam as a solver
4
2.0
Measured X Predicted X - SVR linear
2 1 0 −1 −2 −3 −4 0
Position (normalized)
Position (normalized)
1.0 0.5 0.0 −0.5 −1.0
0 −200
0.0 −0.5 −1.0
50
100 150 200 250 300 350 400 450 Sample Count
−1
Measured Predicted Z - SVR linear
50
100 150 200 250 300 350 400 450 Sample Count
1 0 −1 −2 −3 0
Measured Z Predicted Z - SVR RBF
50
100 150 200 250 300 350 400 450 Sample Count
Fig. 8: Component-wise Support Vector regression on training data set with linear kernel (left) and RBF kernel (right)
−400 −600 0
1
−3 0
1.0 0.5
2
0
−2
Measured Y Predicted Y - SVR RBF
1.5
−2.0 0
100 150 200 250 300 350 400 450 Sample Count
Position (normalized)
200
100 150 200 250 300 350 400 450 Sample Count
−1.5 50
2 Position (normalized)
Magnetic Field Strength (raw ADC)
Raw X Butterworth
50
2.0
−1.5
400
−0.5 −1.0
2.5 Measured Y Predicted Y - SVR linear
1.5
−2.0 0
0.5 0.0
−2.0 0
100 150 200 250 300 350 400 450 Sample Count
2.5
600
1.0
−1.5
50
2.0
Fig. 6: Camera images of the marker and its extracted position during the test trajectory.
Measured X Predicted X - SVR RBF
1.5 Position (normalized)
Position (normalized)
3
50
100 150 200 250 300 350 400 450 Sample Count
Fig. 7: Second order Butterworth filter applied to magnetometer raw measurements
and the rectified linear unit function over the logistic sigmoid and hyperbolic tan functions for activation. For a detailed description of these metaparameters, see [21]–[25]. After training with these meta parameters, the single component SVR models achieved R2 -score [26] of 0.33, 0.73, 0.93 (X, Y, Z linear kernel) and 0.65, 0.97, 0.97 (X, Y, Z RBF kernel), reflected in the fitting curves depicted in figure 8. As expected, the linear kernel is not able to fit the data to the extend of the RBF kernel, due to the inherent non-linearity of the magnetic field measurements. Compared to that, the MLP Regressor with the parameters presented above achieved an overall R2 -score of 0.97 (see figure 9). To further evaluate our trained models, we tried to predict the position from magnetometer readings using data from a totally different trajectory (straight run towards the sensors). As shown in figure 10, both the trained SVR RBF and the MLPR were able to estimate the position given just the magnetometer readings. Although the MLPR had a better R2 score during fitting, it seems that the SVR RBF has better capabilities to generalize or interpolate at regions with no training data, as it shows smaller errors at the beginning of the trajectory. As expected, both SVR and MLPR predictions are getting significantly better, the closer the distance between the magnet and magnetometer.
C. AUV Docking Scenario Even though the complete magnetic-optical docking system was not yet implemented into an AUV, a preliminary experiment outside of the water was conducted in order to evaluate its feasibility. Since the electronics could not yet be wired to the AUV, the data was externally processed. The setup is shown in figure 11. The Docking-Station fitted with both visual marker and magnet was placed in front of the AUV, with a distance of approx. 40cm. It was then manually moved towards the camera until a docking would have been completed in a real docking attempt. the resulting position estimation by the camera and the magnetometers are shown in figure 12. While this is obviously only a qualitative experiment, it helps to undermine the feasibility of proposed localization system in showing that it can provide the necessary navigational data for multi-modal docking. IV. C ONCLUSION In this work an integrated optical/magnetic localization system has been described in detail. The system has been thoroughly tested in a laboratory environment with vehicle testing being in preparation for a docking task using the AUV Leng [5]. The promising results of the bimodal localization approach make an application of this technology for robust close-proximity positioning and docking possible. In our approach two independent measurements have to be combined and processed into a single estimate which can then be used for navigation. This can be done in a number of ways, e.g. a Kalman filter, since it intrinsically can handle missing data from one of the inputs. This step is necessary to avoid
0.20
0.20 Measured X Predicted X - SVR RBF
0.15
Position (m)
Position (m)
0.10
0.05 0.00 −0.05 −0.10 −0.20 0
10
15 20 Sample Count
25
−0.20 0
30
Position (m)
Position (m)
50
10
15 20 Sample Count
25
0.1
5
10
15 20 Sample Count
25
30
Measured Z Predicted Z - MLPR
0.35
0.35 0.25 0.20
0.25 0.20 0.15 0.10
0.15
Measured Y Predicted Y - MLPR
−0.05
0.30
0.30
0.10 0
0.05
0.40
0.40
0.2
Measured Y Predicted Y - MLPR
0.00
−0.20 0
30
Measured Z Predicted Z - SVR RBF
0.45
100 150 200 250 300 350 400 450 Sample Count
30
−0.15 5
Position (m)
−0.4 0
25
−0.10
0.50
Measured X Predicted X - MLPR
Position (m)
Position (m)
−0.05 −0.15
−0.3
Position (m)
0.05
−0.10
−0.2
15 20 Sample Count
0.10
0.00
−0.20 0
10
0.15
0.10
−0.1
5
0.20 Measured Y Predicted Y - SVR RBF
0.15
0.0
−0.05 −0.15
5
0.20
0.1
0.05 0.00 −0.10
−0.15
0.2
Measured X Predicted X - MLPR
0.15
0.10
0.05 5
10
15 20 Sample Count
25
30
0.00 0
5
10
15 20 Sample Count
25
30
Fig. 10: Component-wise position prediction from magnetic field measurements during straight evaluation run, based on trained SVRRBF (left) and MLPR (right) models
0.0 −0.1 −0.2 −0.3 −0.4 0
50
100 150 200 250 300 350 400 450 Sample Count
0.6 Measured Z Predicted Z - MLPR
Position (m)
0.5
(a) AUV Leng with attached mag-(b) Camera view of AUV Leng netometers. during dry docking experiment. Fig. 11: Setup of the dry docking experiment.
0.4 0.3 0.2 0.1 0.0 0
50
100 150 200 250 300 350 400 450 Sample Count
Fig. 9: Component-wise Multi Layer Perceptron regression on training data set, LBFGS solver
modifications of the existing docking procedure, which expects a single position estimate of the docking station relative to the vehicle. For the upcoming future a number of experiments involving real docking scenarios are planned. While the AUV Leng docked with various systems (see figure 13) this has been done using only the visual marker detection without the magnetometers. Once the magnetometers are fully integrated into the AUV, the docking mechanism presented in this work will be used to improve the robustness of the docking process. ACKNOWLEDGMENT The authors would like to thank all colleagues at DFKI GmbH, Robotics Innovation Center, Bremen for their support and feedback. The research leading to this results has received funding from the German Federal Ministry of Economics and Technology (BMWi), projects Europa-Explorer
Fig. 12: Trajectory of the docking-approach during the AUV docking scenario as estimated by marker tracking (red) and magnetometer tracking (black).
Fig. 13: The AUV Leng docking to the ice-shuttle and the SherpaRover.
and TransTerrA (grants No. 50NA1217, 50RA1301). R EFERENCES [1] B. W. Hobson, R. S. McEwen, J. Erickson, T. Hoover, L. McBride, F. Shane, and J. G. Bellingham, “The development and ocean testing of an auv docking station for a 21” auv,” in OCEANS 2007. IEEE, 2007, pp. 1–6. [2] J. Albiez, S. Joyeux, C. Gaudig, J. Hilljegerdes, S. Kroffke, C. Schoo, S. Arnold, G. Mimoso, P. Alcantara, R. Saback et al., “Flatfish-a compact subsea-resident inspection auv,” in OCEANS’15 MTS/IEEE Washington. IEEE, 2015, pp. 1–8. [3] A. Brighenti, L. Zugno, F. Mattiuzzo, and A. Sperandio, “EURODOCKER-A universal docking-downloading-recharging system for AUVs: Conceptual design results,” in in Proc. Oceans 98, Oct, 1998, p. 1463. [4] M. Wirtz, M. Hildebrandt, and C. Gaudig, “Design and test of a robust docking system for hovering AUVs,” in 2012 Oceans. Ieee, Oct. 2012, pp. 1–6. [Online]. Available: http://ieeexplore.ieee.org/lpdocs/ epic03/wrapper.htm?arnumber=6404975 [5] M. Hildebrandt, J. Albiez, M. Wirtz, P. Kloss, J. Hilljegerdes, and F. Kirchner, “Design of an Autonomous Under-Ice Exploration System,” in In MTS/IEEE Oceans 2013 San Diego, (OCEANS-2013). IEEE, 2013, pp. 1–6.
[6] R. M. Eustice, L. L. Whitcomb, H. Singh, and M. Grund, “Experimental Results in Synchronous-Clock One-Way-TravelTime Acoustic Navigation for Autonomous Underwater Vehicles,” Proceedings 2007 IEEE International Conference on Robotics and Automation, pp. 4257–4264, Apr. 2007. [Online]. Available: http: //ieeexplore.ieee.org/lpdocs/epic03/wrapper.htm?arnumber=4209752 [7] M. Feezor, F. Yates Sorrell, P. Blankinship, and J. G. Bellingham, “Autonomous underwater vehicle homing/docking via electromagnetic guidance,” IEEE Journal of Oceanic Engineering, vol. 26, no. 4, pp. 515–521, 2001. [Online]. Available: http://ieeexplore.ieee.org/lpdocs/ epic03/wrapper.htm?arnumber=972086 [8] J. Kim, “Dual Control Approach for Automatic Docking using Monocular Vision,” PHd Dissertation, Stanford University, 2007. [9] A. Murarka, G. Kuhlmann, S. Gulati, C. Flesher, M. Sridharan, and W. C. Stone, “Vision-based frozen surface egress: A docking algorithm for the ENDURANCE AUV,” in Proceedings UUST09, Conference on Un-manned, Un-tethered Submersible Technology, no. December, 2009, pp. 22–25. [Online]. Available: http://www.cs.utexas.edu/∼ai-lab/pubs/ murarka-uust-09.pdf [10] L. Christensen, C. Gaudig, and F. Kirchner, “Distortion-robust distributed magnetometer for underwater pose estimation in confined uuvs,” in OCEANS’15 MTS/IEEE Washington. IEEE, 2015, pp. 1–8. [11] D. B. dos Santos Cesar, C. Gaudig, M. Fritsche, M. A. dos Reis, and F. Kirchner, “An evaluation of artificial fiducial markers in underwater environments,” in OCEANS 2015-Genova. IEEE, 2015, pp. 1–6. [12] E. Olson, “Apriltag: A robust and flexible visual fiducial system,” in Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011, pp. 3400–3407. [13] S. Garrido-Jurado, R. Muoz-Salinas, F. Madrid-Cuevas, and M. MarnJimnez, “Automatic generation and detection of highly reliable fiducial markers under occlusion,” Pattern Recognition, vol. 47, no. 6, pp. 2280 – 2292, 2014. [Online]. Available: http://www.sciencedirect.com/ science/article/pii/S0031320314000235 [14] Chulliat, A., S. Macmillan, P. Alken, C. Beggan, M. Nair, B. Hamilton, A. Woods, V. Ridley, S. Maus, and A. Thomson, “The US/UK world magnetic model for 2015-2020,” NOAA National Geophysical Data Center, Boulder, CO, Tech. Rep., 2015. [15] L. Christensen, M. Krell, and F. Kirchner, “Learning magnetic field distortion compensation for robotic systems,” in Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on, Vancouver, Canada, Sep. 2017, pp. 1–6, in press. [16] M. Caruso, “Applications of magnetic sensors for low cost compass systems,” in Position Location and Navigation Symposium, IEEE 2000, 2000, pp. 177–184. [17] V. Renaudin, M. H. Afzal, and G. Lachapelle, “Complete triaxis magnetometer calibration in the magnetic domain,” Journal of sensors, vol. 2010, 2010. [18] M. Hildebrandt, C. Gaudig, L. Christensen, S. Natarajan, J. H. Carrio, P. M. Paranhos, and F. Kirchner, “A validation process for underwater localization algorithms,” International Journal of Advanced Robotic Systems, vol. 11, 2014. [19] L. Christensen, P. Kampmann, M. Hildebrandt, J. Albiez, and F. Kirchner, “Hardware rov simulation facility for the evaluation of novel underwater manipulation techniques,” in OCEANS 2009-EUROPE. IEEE, 2009, pp. 1–8. [20] F. Pedregosa et al., “Scikit-learn: Machine learning in Python,” Journal of Machine Learning Research, vol. 12, pp. 2825–2830, 2011. [21] V. Vapnik, “The nature of statistical learning theory,” Springer, 2000. [22] A. J. Smola and B. Sch¨olkopf, “A tutorial on support vector regression,” Statistics and Computing, vol. 14, no. 3, pp. 199–222, 2004. [23] C.-C. Chang and C.-J. Lin, “LIBSVM,” ACM Transactions on Intelligent Systems and Technology, vol. 2, no. 3, pp. 1–27, 2011. [24] G. E. Hinton, “Connectionist learning procedures,” Artificial Intelligence, vol. 40, no. 1-3, pp. 185–234, 1989. [25] X. Glorot and Y. Bengio, “Understanding the difficulty of training deep feedforward neural networks,” in Aistats, vol. 9, 2010, pp. 249–256. [26] R. Anderson-Sprecher, “Model comparisons and R2 ,” The American Statistician, vol. 48, no. 2, pp. 113–117, 1994.
