Localization and Mapping for Robots in Agriculture and Forestry: A Survey

Research and development of autonomous mobile robotic solutions that can perform several active agricultural tasks (pruning, harvesting, mowing) have been growing. Robots are now used for a variety of tasks such as planting, harvesting, environmental monitoring, supply of water and nutrients, and others. To do so, robots need to be able to perform online localization and, if desired, mapping. The most used approach for localization in agricultural applications is based in standalone Global Navigation Satellite System-based systems. However, in many agricultural and forest environments, satellite signals are unavailable or inaccurate, which leads to the need of advanced solutions independent from these signals. Approaches like simultaneous localization and mapping and visual odometry are the most promising solutions to increase localization reliability and availability. This work leads to the main conclusion that, few methods can achieve simultaneously the desired goals of scalability, availability, and accuracy, due to the challenges imposed by these harsh environments. In the near future, novel contributions to this field are expected that will help one to achieve the desired goals, with the development of more advanced techniques, based on 3D localization, and semantic and topological mapping. In this context, this work proposes an analysis of the current state-of-the-art of localization and mapping approaches in agriculture and forest environments. Additionally, an overview about the available datasets to develop and test these approaches is performed. Finally, a critical analysis of this research field is done, with the characterization of the literature using a variety of metrics.

A Distributed Radio Beacon/IMU/Altimeter Integrated Localization Scheme with Uncertain Initial Beacon Locations for Lunar Pinpoint Landing

As a growing number of exploration missions have successfully landed on the Moon in recent decades, ground infrastructures, such as radio beacons, have attracted a great deal of attention in the design of navigation systems. None of the available studies regarding integrating beacon measurements for pinpoint landing have considered uncertain initial beacon locations, which are quite common in practice. In this paper, we propose a radio beacon/inertial measurement unit (IMU)/altimeter localization scheme that is sufficiently robust regarding uncertain initial beacon locations. This scheme was designed based on the sparse extended information filter (SEIF) to locate the lander and update the beacon configuration at the same time. Then, an adaptive iterated sparse extended hybrid filter (AISEHF) was devised by modifying the prediction and update stage of SEIF with a hybrid-form propagation and a damping iteration algorithm, respectively. The simulation results indicated that the proposed method effectively reduced the error in the position estimations caused by uncertain beacon locations and made an effective trade-off between the estimation accuracy and the computational efficiency. Thus, this method is a potential candidate for future lunar exploration activities.

Exactly sparse Gaussian variational inference with application to derivative-free batch nonlinear state estimation

We present a Gaussian variational inference (GVI) technique that can be applied to large-scale nonlinear batch state estimation problems. The main contribution is to show how to fit both the mean and (inverse) covariance of a Gaussian to the posterior efficiently, by exploiting factorization of the joint likelihood of the state and data, as is common in practical problems. This is different than maximum a posteriori (MAP) estimation, which seeks the point estimate for the state that maximizes the posterior (i.e., the mode). The proposed exactly sparse Gaussian variational inference (ESGVI) technique stores the inverse covariance matrix, which is typically very sparse (e.g., block-tridiagonal for classic state estimation). We show that the only blocks of the (dense) covariance matrix that are required during the calculations correspond to the non-zero blocks of the inverse covariance matrix, and further show how to calculate these blocks efficiently in the general GVI problem. ESGVI operates iteratively, and while we can use analytical derivatives at each iteration, Gaussian cubature can be substituted, thereby producing an efficient derivative-free batch formulation. ESGVI simplifies to precisely the Rauch–Tung–Striebel (RTS) smoother in the batch linear estimation case, but goes beyond the ‘extended’ RTS smoother in the nonlinear case because it finds the best-fit Gaussian (mean and covariance), not the MAP point estimate. We demonstrate the technique on controlled simulation problems and a batch nonlinear simultaneous localization and mapping problem with an experimental dataset.

GNSS/INS/LiDAR-SLAM Integrated Navigation System Based on Graph Optimization

A Global Navigation Satellite System (GNSS)/Inertial Navigation System (INS)/Light Detection and Ranging (LiDAR)-Simultaneous Localization and Mapping (SLAM) integrated navigation system based on graph optimization is proposed and implemented in this paper. The navigation results are obtained by the information fusion of the GNSS position, Inertial Measurement Unit (IMU) preintegration result and the relative pose from the 3D probability map matching with graph optimizing. The sliding window method was adopted to ensure that the computational load of the graph optimization does not increase with time. Land vehicle tests were conducted, and the results show that the proposed GNSS/INS/LiDAR-SLAM integrated navigation system can effectively improve the navigation positioning accuracy compared to GNSS/INS and other current GNSS/INS/LiDAR methods. During the simulation of one-minute periods of GNSS outages, compared to the GNSS/INS integrated navigation system, the root mean square (RMS) of the position errors in the North and East directions of the proposed navigation system are reduced by approximately 82.2% and 79.6%, respectively, and the position error in the vertical direction and attitude errors are equivalent. Compared to the benchmark method of GNSS/INS/LiDAR-Google Cartographer, the RMS of the position errors in the North, East and vertical directions decrease by approximately 66.2%, 63.1% and 75.1%, respectively, and the RMS of the roll, pitch and yaw errors are reduced by approximately 89.5%, 92.9% and 88.5%, respectively. Furthermore, the relative position error during the GNSS outage periods is reduced to 0.26% of the travel distance for the proposed method. Therefore, the GNSS/INS/LiDAR-SLAM integrated navigation system proposed in this paper can effectively fuse the information of GNSS, IMU and LiDAR and can significantly mitigate the navigation error, especially for cases of GNSS signal attenuation or interruption.

Analytical SLAM without linearization

This paper solves the classical problem of simultaneous localization and mapping (SLAM) in a fashion that avoids linearized approximations altogether. Based on the creation of virtual synthetic measurements, the algorithm uses a linear time-varying Kalman observer, bypassing errors and approximations brought by the linearization process in traditional extended Kalman filtering SLAM. Convergence rates of the algorithm are established using contraction analysis. Different combinations of sensor information can be exploited, such as bearing measurements, range measurements, optical flow, or time-to-contact. SLAM-DUNK, a more advanced version of the algorithm in global coordinates, exploits the conditional independence property of the SLAM problem, decoupling the covariance matrices between different landmarks and reducing computational complexity to O(n). As illustrated in simulations, the proposed algorithm can solve SLAM problems in both 2D and 3D scenarios with guaranteed convergence rates in a full nonlinear context.

Visual EKF-SLAM from Heterogeneous Landmarks

Many applications require the localization of a moving object, e.g., a robot, using sensory data acquired from embedded devices. Simultaneous localization and mapping from vision performs both the spatial and temporal fusion of these data on a map when a camera moves in an unknown environment. Such a SLAM process executes two interleaved functions: the front-end detects and tracks features from images, while the back-end interprets features as landmark observations and estimates both the landmarks and the robot positions with respect to a selected reference frame. This paper describes a complete visual SLAM solution, combining both point and line landmarks on a single map. The proposed method has an impact on both the back-end and the front-end. The contributions comprehend the use of heterogeneous landmark-based EKF-SLAM (the management of a map composed of both point and line landmarks); from this perspective, the comparison between landmark parametrizations and the evaluation of how the heterogeneity improves the accuracy on the camera localization, the development of a front-end active-search process for linear landmarks integrated into SLAM and the experimentation methodology.

Simultaneous Localization and Mapping with Iterative Sparse Extended Information Filter for Autonomous Vehicles

In this paper, a novel iterative sparse extended information filter (ISEIF) was proposed to solve the simultaneous localization and mapping problem (SLAM), which is very crucial for autonomous vehicles. The proposed algorithm solves the measurement update equations with iterative methods adaptively to reduce linearization errors. With the scalability advantage being kept, the consistency and accuracy of SEIF is improved. Simulations and practical experiments were carried out with both a land car benchmark and an autonomous underwater vehicle. Comparisons between iterative SEIF (ISEIF), standard EKF and SEIF are presented. All of the results convincingly show that ISEIF yields more consistent and accurate estimates compared to SEIF and preserves the scalability advantage over EKF, as well.

Quaternion-based EKF-SLAM from relative pose measurements: observability analysis and applications

In this work, we investigate a quaternion-based formulation of 3D Simultaneous Localization and Mapping with Extended Kalman Filter (EKF-SLAM) using relative pose measurements. We introduce a discrete-time derivation that avoids thenormalization problemthat often arises when using unit quaternions in Kalman filter and we study its observability properties. The consistency of the estimation errors with the corresponding covariance matrices is also evaluated. The approach is further tested on real data from theRawseeds datasetand it is applied within a delayed-state EKF architecture for estimating a dense 3D map of an unknown environment. The contribution is motivated by the possibility of abstracting multi-sensorial information in terms of relative pose measurements and for its straightforward extensions to the multi robot case.

A framework for learning semantic maps from grounded natural language descriptions

This paper describes a framework that enables robots to efficiently learn human-centric models of their environment from natural language descriptions. Typical semantic mapping approaches are limited to augmenting metric maps with higher-level properties of the robot’s surroundings (e.g. place type, object locations) that can be inferred from the robot’s sensor data, but do not use this information to improve the metric map. The novelty of our algorithm lies in fusing high-level knowledge that people can uniquely provide through speech with metric information from the robot’s low-level sensor streams. Our method jointly estimates a hybrid metric, topological, and semantic representation of the environment. This semantic graph provides a common framework in which we integrate information that the user communicates (e.g. labels and spatial relations) with metric observations from low-level sensors. Our algorithm efficiently maintains a factored distribution over semantic graphs based upon the stream of natural language and low-level sensor information. We detail the means by which the framework incorporates knowledge conveyed by the user’s descriptions, including the ability to reason over expressions that reference yet unknown regions in the environment. We evaluate the algorithm’s ability to learn human-centric maps of several different environments and analyze the knowledge inferred from language and the utility of the learned maps. The results demonstrate that the incorporation of information from free-form descriptions increases the metric, topological, and semantic accuracy of the recovered environment model.

Semi-parametric learning for visual odometry

This paper addresses the visual odometry problem from a machine learning perspective. Optical flow information from a single camera is used as input for a multiple-output Gaussian process (MOGP) framework, that estimates linear and angular camera velocities. This approach has several benefits. (1) It substitutes the need for conventional camera calibration, by introducing a semi-parametric model that is able to capture nuances that a strictly parametric geometric model struggles with. (2) It is able to recover absolute scale if a range sensor (e.g. a laser scanner) is used for ground-truth, provided that training and testing data share a certain similarity. (3) It is naturally able to provide measurement uncertainties. We extend the standard MOGP framework to include the ability to infer joint estimates (full covariance matrices) for both translation and rotation, taking advantage of the fact that all estimates are correlated since they are derived from the same vehicle. We also modify the common zero mean assumption of a Gaussian process to accommodate a standard geometric model of the camera, thus providing an initial estimate that is then further refined by the non-parametric model. Both Gaussian process hyperparameters and camera parameters are trained simultaneously, so there is still no need for traditional camera calibration, although if these values are known they can be used to speed up training. This approach has been tested in a wide variety of situations, both 2D in urban and off-road environments (two degrees of freedom) and 3D with unmanned aerial vehicles (six degrees of freedom), with results that are comparable to standard state-of-the-art visual odometry algorithms and even more traditional methods, such as wheel encoders and laser-based Iterative Closest Point. We also test its limits to generalize over environment changes by varying training and testing conditions independently, and also by changing cameras between training and testing.

Neural network-based multiple robot simultaneous localization and mapping

In this paper, a decentralized platform for simultaneous localization and mapping (SLAM) with multiple robots is developed. Each robot performs single robot view-based SLAM using an extended Kalman filter to fuse data from two encoders and a laser ranger. To extend this approach to multiple robot SLAM, a novel occupancy grid map fusion algorithm is proposed. Map fusion is achieved through a multistep process that includes image preprocessing, map learning (clustering) using neural networks, relative orientation extraction using norm histogram cross correlation and a Radon transform, relative translation extraction using matching norm vectors, and then verification of the results. The proposed map learning method is a process based on the self-organizing map. In the learning phase, the obstacles of the map are learned by clustering the occupied cells of the map into clusters. The learning is an unsupervised process which can be done on the fly without any need to have output training patterns. The clusters represent the spatial form of the map and make further analyses of the map easier and faster. Also, clusters can be interpreted as features extracted from the occupancy grid map so the map fusion problem becomes a task of matching features. Results of the experiments from tests performed on a real environment with multiple robots prove the effectiveness of the proposed solution.

Autonomous navigation for autonomous underwater vehicles based on information filters and active sensing

This paper addresses an autonomous navigation method for the autonomous underwater vehicle (AUV) C-Ranger applying information-filter-based simultaneous localization and mapping (SLAM), and its sea trial experiments in Tuandao Bay (Shangdong Province, P.R. China). Weak links in the information matrix in an extended information filter (EIF) can be pruned to achieve an efficient approach-sparse EIF algorithm (SEIF-SLAM). All the basic update formulae can be implemented in constant time irrespective of the size of the map; hence the computational complexity is significantly reduced. The mechanical scanning imaging sonar is chosen as the active sensing device for the underwater vehicle, and a compensation method based on feedback of the AUV pose is presented to overcome distortion of the acoustic images due to the vehicle motion. In order to verify the feasibility of the navigation methods proposed for the C-Ranger, a sea trial was conducted in Tuandao Bay. Experimental results and analysis show that the proposed navigation approach based on SEIF-SLAM improves the accuracy of the navigation compared with conventional method; moreover the algorithm has a low computational cost when compared with EKF-SLAM.

A general purpose feature extractor for light detection and ranging data

Feature extraction is a central step of processing Light Detection and Ranging (LIDAR) data. Existing detectors tend to exploit characteristics of specific environments: corners and lines from indoor (rectilinear) environments, and trees from outdoor environments. While these detectors work well in their intended environments, their performance in different environments can be poor. We describe a general purpose feature detector for both 2D and 3D LIDAR data that is applicable to virtually any environment. Our method adapts classic feature detection methods from the image processing literature, specifically the multi-scale Kanade-Tomasi corner detector. The resulting method is capable of identifying highly stable and repeatable features at a variety of spatial scales without knowledge of environment, and produces principled uncertainty estimates and corner descriptors at same time. We present results on both software simulation and standard datasets, including the 2D Victoria Park and Intel Research Center datasets, and the 3D MIT DARPA Urban Challenge dataset.

Experiments with Cooperative Control of Underwater Robots

In this paper we describe cooperative control algorithms for robots and sensor nodes in an underwater environment. Cooperative navigation is defined as the ability of a coupled system of autonomous robots to pool their resources to achieve long-distance navigation and a larger controllability space. Other types of useful cooperation in underwater environments include: exchange of information such as data download and retasking; cooperative localization and tracking; and physical connection (docking) for tasks such as deployment of underwater sensor networks, collection of nodes and rescue of damaged robots. We present experimental results obtained with an underwater system that consists of two very different robots and a number of sensor network modules. We present the hardware and software architecture of this underwater system. We then describe various interactions between the robots and sensor nodes and between the two robots, including cooperative navigation. Finally, we describe our experiments with this underwater system and present data.