This paper presents a novel strategy for offline estimation of the spatial motion of a multi-link mechanism using Inertial Measurement Unit (IMU) sensors. Accelerometers, gyroscopes and magnetometers are strategically mounted and modeled to maximize measurement accuracy through the past work of inertial sensor fusion. The core contribution of this paper is the development of the Factor Graph Optimization (FGO) Preconditioned by the Extended Kalman Filter (EKF), which is termed FGOPreEKF in this paper, and its integration with the inertial sensor fusion. Since the online EKF efficiently derives the initial guess using the same motion and sensor models, the FGO estimates the motion of a multi-link mechanism efficiently and accurately. The proposed approach was experimentally validated on a two-link system mounted on a fast-moving linear axis, demonstrating superior accuracy compared to standalone EKF or FGO. These results demonstrate the potential of this approach for estimating multi-link motion in more complex scenarios.

This article considers simultaneous localization and mapping (SLAM) problem for robots in situations where accurate estimates for some of the robot poses, termed anchors, are available. These may be acquired through external means, for example, by either stopping the robot at some previously known locations or pausing for a sufficient period of time to measure the robot poses with an external measurement system. The main contribution is an efficient algorithm for selecting a fixed number of anchors from a set of potential poses that minimizes estimated error in the SLAM solution. Based on a graph-topological connection between the D-optimality design metric and the tree-connectivity of the pose-graph, the anchor selection problem can be formulated approximately as a submatrix selection problem for reduced weighted Laplacian matrix, leading to a cardinality-constrained submodular maximization problem. Two greedy methods are presented to solve this submodular optimization problem with a performance guarantee. These methods are complemented by Cholesky decomposition, approximate minimum degree permutation, order reuse, and rank-1 update that exploit the sparseness of the weighted Laplacian matrix. We demonstrate the efficiency and effectiveness of the proposed techniques on public-domain datasets, Gazebo simulations, and real-world experiments.

This paper presents a technique for state estimation of a multi-link system having no joint encoders, which can only be partially observed by a camera. To fully observe the system without changing the current configuration, a gyroscope and an accelerometer are attached to each link as dead-reckoning sensors. Observations of the dead-reckoning sensors are associated with the states of the multi-link system such that the states are fully observable. The camera, which observes part of the system globally, is used as a global corrector in the framework of an extended Kalman filter to filter the dead-reckoning errors accumulated over time. Parametric studies in simulation have investigated and identified the efficacy of the proposed technique in estimating the state of the multilink system. Experimental validation using a two-link arm has demonstrated the applicability of the proposed technique to real-world multi-link systems.

Two-dimensional (2-D)/3-D pose-graph simultaneous localization and mapping (SLAM) is a problem of estimating a set of poses based on noisy measurements of relative rotations and translations. This article focuses on the relation between the graphical structure of pose-graph SLAM and Fisher information matrix (FIM), Cramér-Rao lower bounds (CRLB), and its optimal design metrics (T-optimality and D-optimality). As a main contribution, based on the assumption of isotropic Langevin noise for rotation and block-isotropic Gaussian noise for translation, the FIM and CRLB are derived and shown to be closely related to the graph structure, in particular, the weighted Laplacian matrix. We also prove that total node degree and weighted number of spanning trees, as two graph connectivity metrics, are, respectively, closely related to the trace and determinant of the FIM. The discussions show that, compared with the D-optimality metric, the T-optimality metric is more easily computed but less effective. We also present upper and lower bounds for the D-optimality metric, which can be efficiently computed and are almost independent of the estimation results. The results are verified with several well-known datasets, such as Intel, KITTI, sphere, and so on.

The main contribution of this paper is a novel Extended Kalman Filter (EKF) based localisation scheme that fuses two complementary approaches to outdoor vision based localisation. This EKF is aided by a front end consisting of two Convolutional Neural Networks (CNNs) that provide the necessary perceptual information from camera images. The first approach involves a CNN based extraction of information corresponding to artefacts such as curbs, lane markings, and manhole covers to localise on a vector distance transform representation of a binary image of these ground surface boundaries. The second approach involves a CNN based detection of common environmental landmarks such as tree trunks and light poles, which are represented as point features on a sparse map. Utilising CNNs to obtain higher level information about the environment enables this framework to avoid the typical pitfalls of common vision based approaches that use low level hand crafted features for localisation. The EKF framework makes it possible to deal with false positives and missed detections that are inevitable in a practical CNN, to produce a location estimate together with its associated uncertainty. Experiments using a Personal Mobility Device (PMD) driven in typical suburban streets are presented to demonstrate the effectiveness of the proposed localiser.

This paper presents a novel localisation framework based on an omnidirectional camera, targeted at outdoor urban environments. Bearing only information to persistent and easily observable high-level semantic landmarks (such as lamp-posts, street-signs and trees) are perceived using a Convolutional Neural Network (CNN). The framework utilises an information theoretic strategy to decide the best viewpoint to serve as an input to the CNN instead of the full 360° coverage offered by an omnidirectional camera, in order to leverage the advantage of having a higher field of view without compromising on performance. Environmental landmark observations are supplemented with observations to ground surface boundaries corresponding to high-level features such as manhole covers, pavement edges and lane markings extracted from a second CNN. Localisation is carried out in an Extended Kalman Filter (EKF) framework using a sparse 2D map of the environmental landmarks and Vector Distance Transform (VDT) based representation of the ground surface boundaries. This is in contrast to traditional vision only localisation systems that have to carry out Visual Odometry (VO) or Simultaneous Localisation and Mapping (SLAM), since low level features (such as SIFT, SURF, ORB) do not persist over long time frames due to radical appearance changes (illumination, occlusions etc) and dynamic objects. As the proposed framework relies on highlevel persistent semantic features of the environment, it offers an opportunity to carry out localisation on a prebuilt map, which is significantly more resource efficient and robust. Experiments using a Personal Mobility Device (PMD) driven in a representative urban environment are presented to demonstrate and evaluate the effectiveness of the proposed localiser against relevant state of the art techniques.

The main contribution of this paper is a probabilistic estimator that assists a mobile robot to locate a gas source in an indoor environment. The scenario is that a robot equipped with a gas sensor enters a building after the gas is released due to a leak or explosion. The problem is discretized by dividing the environment into a set of regions and time into a set of time intervals. Likelihood functions describing the probability of obtaining a certain gas concentration measurement at a given location at a given time interval are assembled using data generated with GADEN, a three-dimensional gas dispersion simulator([1]). Given a measurement of the gas concentration is available, Bayes's rule is used to compute the joint probability density describing the location of the gas source and the time at which it started spreading. To illustrate the estimation process, a relatively simple motion planner that directs the robot towards the most likely gas source location using a cost function based on the marginal probability of the gas source location is used. The motion plan is periodically revised to reflect the latest posterior probability density. Simulation experiments in a large air-conditioned building with turbulence and wind are presented to demonstrate the effectiveness of the proposed technique.

We present the ANBOT, an intelligent robotic coworker for physical human-robot collaboration. The ANBOT system assists workers performing industrial abrasive blasting, shielding them from the large forces experienced during this physically demanding task. The co-operative robotic system combines the strength and endurance of robots with the decision making of skilled workers. The inherent challenges in human-robot collaboration, combined with the difficult blasting environment required novel design decisions to be made and new solutions to be developed. These include an approach for handling kinematic singularities in a manner suitable for human-robot co-operation, estimating worker pose under poor visibility conditions, and an intuitive control scheme that adapts the robotic assistance based on the estimated strength of the worker. In this work we summarise the ANBOT system and present findings from preliminary site trials. The trials included several real industrial blasting tasks under the control of a skilled abrasive blasting worker who had no experience working alongside a robot. Results demonstrate the suitability of the ANBOT for practical industrial applications.

This paper presents an algorithm for calibrating a “3D range sensor” constructed using a two-dimensional laser range finder (LRF), that is rotated about an axis using a motor to obtain a three-dimensional point cloud. The sensor assembly is modelled as a two degree of freedom open kinematic chain, with one joint corresponding to the axis of the internal mirror in the LRF and the other joint set along the axis of the motor used to rotate the body of the LRF. In the application described in this paper, the sensor unit is mounted on a robot arm used for infrastructure inspection. The objective of the calibration process is to obtain the coordinate transform required to compute the locations of the 3D points with respect to the robot coordinate frame. Proposed strategy uses observations of a set of markers arbitrarily placed in the environment. Distances between these markers are measured and a metric multidimensional scaling is used to obtain the coordinates of the markers with respect to a local coordinate frame. Intensity associated with each beam point of a laser scan is used to locate the reflective markers in the 3D point cloud and a least squares problem is formulated to compute the relationship between the robot coordinate frame, LRF coordinate frame and the marker coordinate frame. Results from experiments using the robot, LRF combination to map a cavity inside a steel bridge structure are presented to demonstrate the effectiveness of the calibration process.

In this paper, we develop a system for the low-cost indoor localization and tracking problem using radio signal strength indicator, Inertial Measurement Unit (IMU), and magnetometer sensors. We develop a novel and simplified probabilistic IMU motion model as the proposal distribution of the sequential Monte-Carlo technique to track the robot trajectory. Our algorithm can globally localize and track a robot with a priori unknown location, given an informative prior map of the Bluetooth Low Energy (BLE) beacons. Also, we formulate the problem as an optimization problem that serves as the Backend of the algorithm mentioned above (Front-end). Thus, by simultaneously solving for the robot trajectory and the map of BLE beacons, we recover a continuous and smooth trajectory of the robot, corrected locations of the BLE beacons, and the time-varying IMU bias. The evaluations achieved using hardware show that through the proposed closed-loop system the localization performance can be improved; furthermore, the system becomes robust to the error in the map of beacons by feeding back the optimized map to the Front-end.

This paper presents a framework for engaging a target while approaching it from a long distance, using observation from sensors on-board a mobile robot. The proposed framework consists of two multi-stage Bayesian approaches to reliably detect and accurately engage with the target under uncertainties. The multi-stage localization approach localizes the robot and the target in a global coordinate frame. Their locations are estimated sequentially when the robot is at a long distance from the target, whereas they are localized simultaneously when the target is in the close vicinity. In the multi-stage target observation approach, a level of confidence and the associated probability of detection of the sensor are defined to make the target detectable in maximal occasions. This allows the extended Kalman filter to be implemented for the target engagement. The proposed framework was implemented on an unmanned ground vehicle equipped with multiple sensors. Results show the effectiveness of the proposed framework in solving real-world problems.

The main contribution of this paper is an extended Kalman filter (EKF)based algorithm for estimating the 6 DOF pose of a camera using monocular images of an indoor environment. In contrast to popular visual simultaneous localisation and mapping algorithms, the technique proposed relies on a pre-built map represented as an unsigned distance function of the ground plane edges. Images from the camera are processed using a Convolutional Neural Network (CNN)to extract a ground plane edge image. Pixels that belong to these edges are used in the observation equation of the EKF to estimate the camera location. Use of the CNN makes it possible to extract ground plane edges under significant changes to scene illumination. The EKF framework lends itself to use of a suitable motion model, fusing information from any other sensors such as wheel encoders or inertial measurement units, if available, and rejecting spurious observations. A series of experiments are presented to demonstrate the effectiveness of the proposed technique.

This paper presents a novel appearance and shape feature, RISAS, which is robust to viewpoint, illumination, scale and rotation variations. RISAS consists of a keypoint detector and a feature descriptor both of which utilise texture and geometric information present in the appearance and shape channels. A novel response function based on the surface normals is used in combination with the Harris corner detector for selecting keypoints in the scene. A strategy that uses the depth information for scale estimation and background elimination is proposed to select the neighbourhood around the keypoints in order to build precise invariant descriptors. Proposed descriptor relies on the ordering of both grayscale intensity and shape information in the neighbourhood. Comprehensive experiments which confirm the effectiveness of the proposed RGB-D feature when compared with CSHOT [1] and LOIND[2] are presented. Furthermore, we highlight the utility of incorporating texture and shape information in the design of both the detector and the descriptor by demonstrating the enhanced performance of CSHOT and LOIND when combined with RISAS detector.

This work presents a real-time localization system developed for professional care givers to track residents of an aged care facility during their visits to a crowded, multi-story shopping mall. The proposed system consists of a Wi-Fi based self-localization platform integrated into a wheeled walking frame and an application installed in a hand-held tablet device for displaying the locations of walker users. The density of people in the shopping mall changes significantly during the day thus the expected Wi-Fi signal strength at a given location is subject to large variations. However, Identifying the location to be within a given area is adequate and the average speed of motion is less than 0.5 m/sec. In this paper, an algorithm that addresses these unique requirements is presented. We exploit the signal strength characteristics of existing Wi-Fi network and prior knowledge of the building floor plans for developing our core algorithm. The environments is divided in to cells that are either enclosed spaces or divisions of larger open regions. The probability density function of the Wi-Fi signal strength of each cell is estimated using Kernel Density Estimation (KDE) and is used in a probabilistic framework to estimate the user location. Motion model of the users as well as the detection of floor transition events are used to enhance the performance of the location estimator. The algorithm was implemented using an Odroid-C1 computer and a tablet with Android operating system. Results obtained during field trials at Roselands Shopping Mall in Sydney are presented.

This paper presents a generalized multi-stage Bayesian approach for an unmanned aerial vehicle to estimate the location of a mobile target. The major hardware components of the proposed approach are a camera with a fisheye lens and another camera with a normal lens and a pan/tilt unit. With wide angle of view (AOV), the fisheye lens camera first detects the bearing of the target, and the PT camera next captures the target in its AOV. The recursive Bayesian estimation steadily locates the target in a globally defined space. The paper also proposes a multi-stage detection method for the fisheye lens camera. The level of confidence is defined in association with the probability of detection (POD) for each detection technique, and the fisheye lens enables continuous detection by gradually increasing the POD. The observation likelihood is finally derived from the POD in a generalized manner. The proposed approach was applied to the detection of a mobile target by a multi-rotor helicopter, and results have demonstrated the effectiveness of both the proposed multi-stage Bayesian approach and multi-stage fisheye lens detection method.

The main contribution of this paper is a planar scan matching algorithm that makes use of the incident angle of a scan point as a feature to enhance the robustness to large relative transformations, particularly in orientation. A new definition of the incident angle is introduced and its consistency with respect to relative transformation between scans is demonstrated. A method that uses the Radial distance and Incident Angle (RIA) for scan alignment, that can efficiently estimate the relative transformation between two scans, is proposed. This approach is designed as a preprocessor for fine scan matching and it is able to pre-align scans that have large angular but limited translational displacement. Scans collected with aggressive robot motion in two typical indoor environments as well as benchmark datasets, are used to demonstrate the effectiveness of RIA when compared with a range of scan matching algorithms reported in the literature.

This paper presents a novel approach to metric scale reconstruction of a three-dimensional (3D) scene using a monocular camera. Using a sequence of images from a monocular camera with a fixed focus lens, metric distance to a set of features in the environment is estimated from image blur due to defocus. The blur texture ambiguity which causes scale errors in depth from defocus is corrected in an EKF framework that exploits image velocity measurements. We show in real experiments that our method converges to a metric scale, accurate, sparse depth map and 3D camera poses with images from a monocular camera. Therefore, the proposed approach has the potential to enhance robot navigation algorithms that rely on monocular cameras.

This paper presents an algorithm for localizing an unmanned aerial vehicle (UAV) in GPS denied environments. Localization is performed with respect to a pre-built map of the environment represented using the distance function of a binary mosaic, avoiding the need for extraction and explicit matching of visual features. Edges extracted from images acquired by an on-board camera are projected to the map to compute an error metric that indicates the misalignment between the predicted and true pose of the UAV. A constrained extended Kalman filter (EKF) framework is used to generate an estimate of the full 6-DOF location of the UAV by enforcing the condition that the distance function values are zero when there is no misalignment. Use of an EKF also makes it possible to seamlessly incorporate information from any other system on the UAV, for example, from its auto-pilot, a height sensor or an optical flow sensor. Experiments using a hexarotor UAV both in a simulation environment and in the field are presented to demonstrate the effectiveness of the proposed algorithm.

The main contribution of this paper is an invariant extended Kalman filter (EKF) for visual inertial navigation systems (VINS). It is demonstrated that the conventional EKF based VINS is not invariant under the stochastic unobservable transformation, associated with a translation and a rotation about the gravitational direction. This can lead to inconsistent state estimates as the estimator does not obey a fundamental property of the physical system. To address this issue, we use a novel uncertainty representation to derive a Right Invariant error extended Kalman filter (RIEKF-VINS) that preserves this invariance property. RIEKF-VINS is then adapted to the multi-state constraint Kalman filter framework to obtain a consistent state estimator. Both Monte Carlo simulations and real-world experiments are used to validate the proposed method.

In this paper, we tackle the direct and inverse problems for the Remote-Field Eddy-Current (RFEC) technology. The direct problem is the sensor model, where given the geometry the measurements are obtained. Conversely, the inverse problem is where the geometry needs to be estimated given the field measurements. These problems are particularly important in the field of Non-Destructive Testing (NDT) because they allow assessing the quality of the structure monitored. We solve the direct problem in a parametric fashion using Least Absolute Shrinkage and Selection Operation (LASSO). The proposed inverse model uses the parameters from the direct model to recover the thickness using least squares producing the optimal solution given the direct model. This study is restricted to the 2D axisymmetric scenario. Both, direct and inverse models, are validated using a Finite Element Analysis (FEA) environment with realistic pipe profiles.

The main contribution of this paper is a high frequency, low-complexity, on-board visual-inertial odometry system for quadrotor micro air vehicles. The system consists of an extended Kalman filter (EKF) based state estimation algorithm that fuses information from a low cost MEMS inertial measurement unit acquired at 200Hz and VGA resolution images from a monocular camera at 50Hz. The dynamic model describing the quadrotor motion is employed in the estimation algorithm as a third source of information. Visual information is incorporated into the EKF by enforcing the epipolar constraint on features tracked between image pairs, avoiding the need to explicitly estimate the location of the tracked environmental features. Combined use of the dynamic model and epipolar constraints makes it possible to obtain drift free velocity and attitude estimates in the presence of both accelerometer and gyroscope biases. A strategy to deal with the unobservability that arises when the quadrotor is in hover is also provided. Experimental data from a real-time implementation of the system on a 50 gram embedded computer are presented in addition to the simulations to demonstrate the efficacy of the proposed system.

Simultaneous localization and mapping (SLAM) in robotics, and a number of related problems that arise in sensor networks are instances of estimation problems over weighted graphs. This paper studies the relation between the graphical representation of such problems and estimationtheoretic concepts such as the Cramér-Rao lower bound (CRLB) and D-optimality. We prove that the weighted number of spanning trees, as a graph connectivity metric, is closely related to the determinant of CRLB. This metric can be efficiently computed for large graphs by exploiting the sparse structure of underlying estimation problems. Our analysis is validated using experiments with publicly available pose-graph SLAM datasets.

This work exploits modeling spatial correlation in 2.5D data using Gaussian Processes (GPs), and produces constrained sampling realizations on these models to improve certainty in the predictions by means of integrating additional sparse information. Data organized in 2.5D such as elevation and thickness maps has been extensively studied in the fields of robotics and geostatistics. These maps are typically represented as a probabilistic 2D grid that stores an estimated value (height or thickness) for each cell. With the increasing popularity and deployment of robotic devices for infrastructure inspection, 2.5D data becomes a common interpretation of the condition of the target being inspected. Modeling the spatial dependencies and making inferences on new grid locations is a common task that has been addressed using GPs, but inference results on locations which are weakly correlated with the training data are generally not sufficiently informative and distinctly uncertain. The predictive capability of the proposed framework, which is applicable to any 2.5D data, is demonstrated with field inspection data from pipelines. Specifically, sparse and complementary measurements from alternative sensing modalities have been incorporated into the model to predict in more detail local thickness conditions where GP training data is limited. The output of this work aims to probabilistically present variations of the target in the case that both accuracy and reasonable diversity are of significant interest.

This paper describes a non-field-of-view (NFOV) localization approach for a mobile robot in an unknown environment based on an acoustic signal combined with the geometrical information from an optical sensor. The approach estimates the location of a target through the mobile robot's sensor observation frame, which consists of a combination of diffraction and reflection acoustic signals and a 3-D environment geometrical description. This fusion of audio-visual sensor observation likelihoods allows the robot to estimate the NFOV target. The diffraction and reflection observations from the microphone array generate the acoustic joint observation likelihood. The observed geometry also determines far-field or near-field acoustic conditions to improve the estimation of the sound direction of arrival. A mobile robot equipped with a microphone array and an RGB-D sensor was tested in a controlled environment, an anechoic chamber, to demonstrate the NFOV localization capabilities. This resulted in +/- 18 degrees, and less than 0.75 m error in angle and distance estimation, respectively.

We propose a scalable algorithm to take advantage of the separable structure of simultaneous localization and mapping (SLAM). Separability is an overlooked structure of SLAM that distinguishes it from a generic nonlinear least-squares problem. The standard relative-pose and relative-position measurement models in SLAM are affine with respect to robot and features' positions. Therefore, given an estimate for robot orientation, the conditionally optimal estimate for the rest of the state variables can be easily computed by solving a sparse linear least-squares problem. We propose an algorithm to exploit this intrinsic property of SLAM by stripping the problem down to its nonlinear core, while maintaining its natural sparsity. Our algorithm can be used in conjunction with any Newton-based solver and is applicable to 2-D/3-D pose-graph and feature-based SLAM. Our results suggest that iteratively solving the nonlinear core of SLAM leads to a fast and reliable convergence as compared to the state-of-the-art sparse back-ends.

This paper presents an active object recognition and pose estimation system for household objects in a highly cluttered environment. A sparse feature model, augmented with the characteristics of features when observed from different viewpoints is used for recognition and pose estimation while a dense point cloud model is used for storing geometry. This strategy makes it possible to accurately predict the expected information available during the Next-Best-View planning process as both the visibility as well as the likelihood of feature matching can be considered simultaneously. Experimental evaluations of the active object recognition and pose estimation with an RGB-D sensor mounted on a Turtlebot are presented.

The problem of active perception with an autonomous robot is studied in this paper. It is proposed that the exploratory behavior of the robot be controlled using mutual information (MI) surfaces between the current map and a one-step look ahead measurements. MI surfaces highlight informative areas for exploration. A novel method for computing these surfaces is described. An approach that exploits structural dependencies of the environment and handles sparse sensor measurements to build a continuous model of the environment, that can then be used to generate MI surfaces is also proposed. A gradient field of occupancy probability distribution is regressed from sensor data as a Gaussian Process and provide frontier boundaries for further exploration. The continuous global frontier surface completely describes unexplored regions and, inherently, provides an automatic termination criterion for a desired sensitivity. The results from publicly available datasets confirm an average improvement of the proposed methodology over comparable standard and state-of-the-art exploratory methods available in the literature by more than 20% and 13% in travel distance and map entropy reduction rate, respectively.

When multiple industrial robots are deployed in field applications such as grit blasting and spray painting of steel bridges, the environments are unstructured for robot operation and the robot positions may not be arranged accurately. Coordination of these multiple robots to maximize productivity through area partitioning and allocation is crucial. This paper presents a novel approach to area partitioning and allocation by utilizing multiobjective optimization and voronoi partitioning. Multiobjective optimization is used to minimize: (1) completion time, (2) proximity of the allocated area to the robot, and (3) the torque experienced by each joint of the robot during task execution. Seed points of the voronoi graph for voronoi partitioning are designed to be the design variables of the multiobjective optimization algorithm. Results of three different simulation scenarios are presented to demonstrate the effectiveness of the proposed approach and the advantage of incorporating robots' torque capacity.

This paper presents a distributed spatial estimation and prediction approach to address the centrally-computed scheme of Gaussian Process regression at each robotic sensor in resource-constrained networks of mobile, wireless and noisy agents monitoring physical phenomena of interest. A mobile sensor independently estimate its own parameters using collective measurements from itself and local neighboring agents as they navigate through the environment. A spatially-distributed prediction algorithm is designed utilizing methods of Jacobi over-relaxation and discrete-time average consensus to enable a robotic sensor to update its estimation of obtaining the global model parameters and recursively compute the global goal of inference. A distributed navigation strategy is also considered to drive sensors to the most uncertain locations enhancing the quality of prediction and learning parameters. Experimental results in a real-world data set illustrate the effectiveness of the proposed approach and is highly comparable to those of the centralized scheme.

This paper proposes a data driven machine learning model for spatial prediction of hydrogen sulfide (H2S) in a gravity sewer system. The gaseous H2S in the overhead of the gravity sewer is modelled using a Gaussian Process with a new covariance function due to constraints of sewer boundaries. The covariance function is proposed based on the distance between two locations computed along the lengths of the sewer network. A mutual information based strategy is used to choose the best k sensor measurements and their locations from among n potential sensor observations and their locations. This provably NP-hard combinatorial sensor selection problem is addressed by maximizing the mutual information between the selected locations and the locations that are not selected or do not have any sensor deployments. A proof-of-concept study was carried out comparing the spatial prediction of H2S with a complex model currently used by Sydney Water. The proposed approach is shown to be effective in both modelling and predicting the H2S spatial concentrations in sewers as well as identifying optimal number of H2S sensors and their locations for a required level of prediction accuracy.

This paper presents a modular algorithm pipeline for recognizing textured household objects in cluttered environment and estimating 6 DOF poses using an RGB-D sensor. The method draws from recent advances in this area and introduces a number of innovations that enable improved performances and faster operational speed in comparison with the state-of-the-art. The pipeline consists of (i) support plane subtraction (ii) SIFT feature extraction and approximate nearest neighbour based matching (iii) feature clustering using 3D Eculidean distances (iv) SVD based pose estimation in combination with a outlier rejection strategy named SORSAC (Spatially ORdered RAndom Consensus) and (v) a pose combination and refinement step to combine overlapping identical instances and to refine the pose estimation result by removing incorrect hypothesis. Quantitative comparisons with the MOPED [1] system on self-constructed dataset are presented to demonstrate the effectiveness of the pipeline.

The problem of inferring human behaviour is naturally complex: people interact with the environment and each other in many different ways, and dealing with the often incomplete and uncertain sensed data by which the actions are perceived only compounds the difficulty of the problem. In this paper, we propose a framework whereby these elaborate behaviours can be naturally simplified by decomposing them into smaller activities, whose temporal dependencies can be more efficiently represented via probabilistic hierarchical learning models. In this regard, patterns of a number of activities typically carried out by users of an ambulatory aid device have been identified with the aid of a Hierarchical Hidden Markov Model (HHMM) framework. By decomposing the complex behaviours into multiple layers of abstraction the approach is shown capable of modelling and learning these tightly coupled human-machine interactions. The inference accuracy of the proposed model is proven to compare favourably against more traditional discriminative models, as well as other compatible generative strategies to provide a complete picture that highlights the benefits of the proposed approach, and opens the door to more intelligent assistance with a robotic mobility aid.

This paper presents a linear approach for solving monocular simultaneous localization and mapping (SLAM) problems. The algorithm first builds a sequence of small initial submaps and then joins these submaps together in a divide-and-conquer (D&C) manner. Each of the initial submap is built using three monocular images by bundle adjustment (BA), which is a simple nonlinear optimization problem. Each step in the D&C submap joining is solved by a linear least squares together with a coordinate and scale transformation. Since the only nonlinear part is in the building of the initial submaps, the algorithm makes it possible to solve large-scale monocular SLAM while avoiding issues associated with initialization, iteration, and local minima that are present in most of the nonlinear optimization based algorithms currently used for large-scale monocular SLAM. Experimental results based on publically available datasets are used to demonstrate that the proposed algorithms yields solutions that are very close to those obtained using global BA starting from good initial guess.

This article proposes a probabilistic approach to account for robot stability uncertainty when planing motions over uneven terrains. A novel probabilistic stability criterion derived from the cumulative distribution of a tip-over metric is introduced that allows a safety constraint to be dynamically updated by available sensor data as it becomes available. The proposed safety constraint authorizes the planner to generates more conservative motion plans for areas with higher levels of uncertainty, while avoids unnecessary caution in well-known areas. The proposed systematic approach is particularly applicable to reconfigurable robots that can assume safer postures when required, although is equally valid for fixed-configuration platforms to choose safer paths to follow. The advantages of planning with the proposed probabilistic stability metric are demonstrated with data collected from an indoor rescue arena, as well as an outdoor rover testing facility.

This paper extends the recently developed Model-Aided Visual-Inertial Fusion (MA-VIF) technique for quadrotor Micro Air Vehicles (MAV) to deal with wind disturbances. The wind effects are explicitly modelled in the quadrotor dynamic equations excluding the unobservable wind velocity component. This is achieved by a nonlinear observability of the dynamic system with wind effects. We show that using the developed model, the vehicle pose and two components of the wind velocity vector can be simultaneously estimated with a monocular camera and an inertial measurement unit. We also show that the MA-VIF is reasonably tolerant to wind disturbances, even without explicit modelling of wind effects and explain the reasons for this behaviour. Experimental results using a Vicon motion capture system are presented to demonstrate the effectiveness of the proposed method and validate our claims.

SLAM can be viewed as an estimation problem over graphs. It is well known that the topology of each dataset affects the quality of the corresponding optimal estimate. In this paper we present a formal analysis of the impact of graph structure on the reliability of the maximum likelihood estimator. In particular, we show that the number of spanning trees in the graph is closely related to the D-optimality criterion in experimental design. We also reveal that in a special class of linear-Gaussian estimation problems over graphs, the algebraic connectivity is related to the E-optimality design criterion. Furthermore, we explain how the average node degree of the graph is related to the ratio between the minimum of negative log-likelihood achievable and its value at the ground truth. These novel insights give us a deeper understanding of the SLAM problem. Finally we discuss two important applications of our analysis in active measurement selection and graph pruning. The results obtained from simulations and experiments on real data confirm our theoretical findings.

This paper addresses the issue of monitoring physical spatial phenomena of interest utilizing the information collected by a network of mobile, wireless and noisy sensors that can take discrete measurements as they navigate through the environment. The spatial phenomenon is statistically modelled by a Gaussian Markov Random Field (GMRF) with hyperparameters that are learnt as the measurements accumulate over time. In this context, the GMRF approximately represents the spatial field on an irregular lattice of triangulation by exploiting a stochastic partial differential equation (SPDE) approach, which benefits remarkably in computation due to the sparsity of the precision matrix. A technique of the one-step-ahead forecast is employed to predict the future measurements that are required to find the optimal sampling locations. It is shown that optimizing the sampling path problem with the logarithm of the determinant either of a covariance matrix using a GP model or of a precision matrix using a GMRF model for mobile robotic wireless sensor networks (MRWSNs) even by a greedy algorithm is impractical. This paper proposes an efficient novel optimality criterion for the adaptive sampling strategy to find the most informative locations in taking future observations that minimize the uncertainty at unobserved locations. The computational complexity of our proposed method is linear, which makes the MRWSN scalable and practically feasible. The effectiveness of the proposed approach is compared and demonstrated using a pre-published data set with appealing results.

This paper presents a novel algorithm for fusing monocular vision and inertial information for quadrotor Micro Air Vehicles by incorporating the unique dynamic characteristics of that platform into the state estimation process. The dynamics of a quadrotor is unique in that a dual axis accelerometer mounted parallel to the propeller plane provides measurements that are directly proportional to vehicle velocities in that plane. By exploiting these dynamic characteristics, we show that all vehicle states, including the absolute scale, become observable in all motion patterns. This distinguishes our method with other visual-inertial fusion methods, which either assume zero accelerometer bias, or require “sufficiently exciting” motion, such as non-zero acceleration, to ensure observability of the scale. The advantages of our method over existing visual-inertial fusion algorithms are proved through a theoretical analysis using Lie Derivatives and verified by extensive simulations and experiments.

Stability prediction is an important concern for mobile robots operating in rough environments. Having the capacity to predict areas of instability means pro-actively being able to plan safer traversable paths. The most influential tip-over stability measures are based on two criteria, the robot's center of mass (CM) and the supporting polygon (SP) defined by the convex area spanned between the ground contact-points. However, there is significant uncertainty associated with many parameters in the planning pipe-line: the actual robot kino-dynamic model, its localisation in the ground, and the terrain models, particularly in uneven terrain. This article proposes a statistical analysis of stability prediction to account for some of the uncertainties. This is accomplished using the force angle (FA) stability measure for a reconfigurable multi-tracked vehicle fitted with flippers, a manipulator arm and a sensor head. Probability density function (PDF) of contact-points, CM and the FA stability measure are numerically estimated, with simulation results performed on the open dynamics engine (ODE) simulator based on uncertain parameters. Two techniques are presented: a conventional Monte Carlo scheme, and a structured unscented transform (UT) which results in significant improvement in computational efficiency. Experimental results on maps obtained from a range camera fitted on the sensor head while the robot traverses over a ramp and a series of steps are presented that confirms the validity of the proposed probabilistic stability prediction method.

This paper presents an approach for exploring a complex 3D environment with a sensor mounted on the end effector of a robot manipulator. In contrast to many current approaches which plan as far ahead as possible using as much environment information as is available, our approach considers only a small set of poses (vector of joint angles) neighbouring the robot's current pose in configuration space. Our approach is compared to an existing exploration strategy for a similar robot. Our results demonstrate a significant decrease in the number of information gain estimation calculations that need to be performed, while still gathering an equivalent or increased amount of information about the environment.

In this paper, the problem of localising a robot within a known two-dimensional environment is formulated as one of minimising the Chamfer Distance between the corresponding occupancy grid map and information gathered from a sensor such as a laser range finder. It is shown that this nonlinear optimisation problem can be solved efficiently and that the resulting localisation algorithm has a number of attractive characteristics when compared with the conventional particle filter based solution for robot localisation in occupancy grids. The proposed algorithm is able to perform well even when robot odometry is unavailable, insensitive to noise models and does not critically depend on any tuning parameters. Experimental results based on a number of public domain datasets as well as data collected by the authors are used to demonstrate the effectiveness of the proposed algorithm.

This paper presents a strategy for large-scale SLAM through solving a sequence of linear least squares problems. The algorithm is based on submap joining where submaps are built using any existing SLAM technique. It is demonstrated that if submaps coordinate frames are judiciously selected, the least squares objective function for joining two submaps becomes a quadratic function of the state vector. Therefore, a linear solution to large-scale SLAM that requires joining a number of local submaps either sequentially or in a more efficient Divide and Conquer manner, can be obtained. The proposed Linear SLAM technique is applicable to both feature-based and pose graph SLAM, in two and three dimensions, and does not require any assumption on the character of the covariance matrices or an initial guess of the state vector. Although this algorithm is an approximation to the optimal full nonlinear least squares SLAM, simulations and experiments using publicly available datasets in 2D and 3D show that Linear SLAM produces results that are very close to the best solutions that can be obtained using full nonlinear optimization started from an accurate initial value. The C/C++ and MATLAB source codes for the proposed algorithm are available on OpenSLAM.

A multiobjective optimization model is presented in this paper for the Autonomous Straddle Carriers Scheduling (ASCS) problem in automated container terminals, which is more practical than the single objective model. The model considers three objectives [i.e., Straddle Carriers (SCs) traveling time, SC waiting time and finishing time of high-priority container-transferring jobs], and their weighted sum is investigated as the representative example. The presented model is formulated as a pickup and delivery problem with time windows in the form of binary integer programming. An exact algorithm based on Branch-and-Bound with Column Generation (BBCG) is employed for solving the multiobjective ASCS problem. Based on the map of an actual fully automated container terminal, simulation results are compared with the single-objective scheduling to demonstrate the effectiveness and flexibility of the presented multiobjective model, as well as the efficacy of the BBCG algorithm for autonomous SC scheduling.

This paper presents a new state vector and a map joining algorithm for range-only SLAM problems. Local maps are built by least squares optimization using the new state vector and a landmark initialization strategy which is an improvement on our preliminary work [1]. The map joining algorithm combines the local maps using least squares optimization to maintain the estimation consistency. Both the local map building and the map joining algorithm maintain a list of “unused range observations” to minimize the potential for information loss. The accuracy of the proposed method is evaluated using a simulation dataset, and an experimental dataset provided by the Robotics Institute at Carnegie Mellon University (CMU).

This paper compares the convergence of least squares based 2D bearing-only SLAM algorithms using different landmark parametrizations. It is shown that the requirement on the accuracy of the initial value vary significantly when using different landmark parametrizations. Especially, for small scale bearing-only SLAM problems, the region of attraction of the global minimum for Gauss-Newton iteration based bearing-only SLAM algorithm using parallax angle landmark parametrization is significantly larger as compared with those of bearing-only SLAM algorithms using other landmark parametrizations.

Understanding the environment in both geometric and semantic levels enables a robot to perform high-level tasks in complex environments. Therefore in recent years research towards identifying and semantically labeling the environments based on onboard sensors for mobile robots has been gaining popularity. After the era of heuristic and rule-based approaches, supervised learning algorithms like Support Vector Machines (SVM) and AdaBoost have been extensively used for this purpose showing satisfactory performance. With the introduction of graphical models, approaches like Conditional Random Fields (CRF) which take the advantage of connectivity of samples provide more flexibility to capture complex dependencies. In this paper, we focus on a real-world task which challenges the generalization ability of the model, evaluate some graph based features, propose a semi-supervised learning algorithm by iteratively utilizing the results from SVM and CRF, and suggest a solution for CRF parameter estimation with partially labeled training data. Experiments have been conducted on six real-world indoor environments demonstrating the competence of the algorithm.

It is now well known that increasing the number of features maintained in the mapping process of the monocular SLAM improves the accuracy of the outcome. This, however, increases the state dimension and the associated computational cost. This paper investigates and evaluates the improvement on SLAM results by exploiting camera motion information. For a camera mounted on a vehicle, its motion is subject to the vehicle motion model. The work of this paper shows that by introducing relative pose constraints calculated from image points by considering the underlying vehicle motion model (for example the non-holonomic vehicle motion model), it is possible to incorporate vehicle motion information into the system and achieve even more accurate SLAM results than maintaining all extracted features in the map. It is demonstrated that in this process, the state dimension is not increased, and the sparse structure of the SLAM problem is maintained. So the underlying sparseness in the SLAM problem structure can still be exploited for computational efficiency. Simulation and experiment results are presented to demonstrate the relative merits of incorporating vehicle motion information for motion estimation and mapping.

Map joining is an efficient strategy for solving feature based SLAM problems. This paper demonstrates that joining of two 2D local maps, formulated as a nonlinear least squares problem has at most two local minima, when the associated uncertainties can be described using spherical covariance matrices. Necessary and sufficient condition for the existence of two minima is derived and it is shown that more than one minimum exists only when the quality of the local maps used for map joining is extremely poor. The analysis explains to some extent why a number of optimization based SLAM algorithms proposed in the recent literature that rely on local search strategies are successful in converging to the globally optimal solution from poor initial conditions, particularly when covariance matrices are spherical. It also demonstrates that the map joining problem has special properties that may be exploited to reliably obtain globally optimal solutions to the SLAM problem.

Discrete choice experiments have traditionally focused on improving the prediction of static choices that are measured through external reflection and surveys. It is argued that considering the underlying processes of decision making across a variety of contexts may further progress decision research. As a pilot study in this field, this paper explores the dynamic nature of decision-making by examining the associated brain activity, Electroencephalogram (EEG), of people while undertaking choices designed to elicit their preferences. To facilitate such a study, the Tobii-Studio eye tracker system was utilized to capture the participants' choice based preferences when they were observing seventy two sets of objects of three images offering potential personal computer backgrounds. Choice based preferences were identified by having the respondent click on their preferred image. In addition, the commercial Emotiv wireless EEG headset with 14 channels was utilized to capture the associated brain activity during the period of the experiments. Principal Component Analysis (PCA) was utilized to preprocess the EEG data before analyzing it with the Fast Fourier Transform (FFT) to observe the changes in the four principal frequency bands, theta (3 - 7 Hz), alpha (8 - 12 Hz), beta (13 - 30 Hz), and gamma (30 - 40 Hz). A mutual information (MI) measure was then used to study left-to-right hemisphere differences as well as front-to-back difference. Across six recruited participants there was a clear and significant change in the spectral activities taking place mainly in the frontal (theta and alpha across F3 and F4) and occipital (alpha and beta across O1 and O2) regions while the participants were indicating their preferences.

Recently RGB-D sensors have become very popular in the area of Simultaneous Localisation and Mapping (SLAM). The major advantage of these sensors is that they provide a rich source of 3D information at relatively low cost. Unfortunately, these sensors in their current forms only have a range accuracy of up to 4 metres. Many techniques which perform SLAM using RGB-D cameras rely heavily on the depth and are restrained to office type and geometrically structured environments. In this paper, a switching based algorithm is proposed to heuristically choose between RGB-BA and RGBD-BA based local maps building. Furthermore, a low cost and consistent optimisation approach is used to join these maps. Thus the potential of both RGB and depth image information are exploited to perform robust SLAM in more general indoor cases. Validation of the proposed algorithm is performed by mapping a large scale indoor scene where traditional RGB-D mapping techniques are not possible.

This paper demonstrates that 2D pose SLAM has an underlining near convex structure when formulated as a least squares (LS) optimization problem. By introducing new variables and some approximations, the LS pose SLAM problem can be formulated as a quadratically constrained quadratic programming (QCQP) problem. The QCQP formulation can then be relaxed into a semi-definite programming (SDP) problem which is convex. Unique solution to the convex SDP problem can be obtained without initial state estimate and can be used to construct a candidate solution to the original LS pose SLAM problem. Simulation datasets and the Intel Research Lab dataset have been used to demonstrate that when the relative pose information contain noises with reasonable level, the candidate solution obtained through the relaxation is very close to the optimal solution to the LS SLAM problem. Thus in practice, the candidate solution can serve as either an approximate solution or a good initial guess for a local optimization algorithm to obtain the optimal solution to the LS pose SLAM problem.

This paper proposes a motion planning strategy for reconfigurable mobile robots in uneven terrain. Paths that guarantee stability while at the same time maximise the height of the sensor payload, thereby enhancing the capacity of the robot to explore the environment are obtained using a search algorithm based on A*. This is particularly applicable to operations such as search and rescue where observing the environment for locating victims is the major objective, although the proposed technique can be generalised to incorporate other potentially conflicting objectives such as minimising energy. The proposed planning strategy looks at exploiting the (possibly incomplete) environment information available to the robot and/or operator as it explores novel terrain. The effectiveness of the approach is evaluated using data obtained from a multi-tracked robot fitted with a manipulator arm and a range camera in a mock-up search and rescue arena.

Representation of spaces including both geometric and semantic information enables a robot to perform high-level tasks in complex environments. Therefore, in recent years identifying and semantically labeling the environments based on onboard sensors has become an important competency for mobile robots. Supervised learning algorithms have been extensively used for this purpose with SVM-based solutions showing good generalization properties. The CRF-based approaches take the advantage of connectivity information of samples thereby provide a mechanism to capture complex dependencies. Blending the complementary strengths of Support Vector Machine (SVM) and Conditional Random Field (CRF), there have been algorithms to exploit the advantages of both to enhance the overall accuracy of place classification in indoor environments. However, experiments show that none of the above approaches deal well with diversified testing data. In this paper, we focus mainly on the generalization ability of the model and propose a semi-supervised learning strategy, which essentially improves the performance of the system. Experiments have been carried out on six real-world maps from different universities around the world and the results from rigorous testing demonstrate the feasibility of the approach.

We present an active exploration strategy that complements Pose SLAM [1] and optimal navigation in Pose SLAM [2]. The method evaluates the utility of exploratory and place revisiting sequences and chooses the one that minimizes overall map and path entropies. The technique considers trajectories of similar path length taking marginal pose uncertainties into account. An advantage of the proposed strategy with respect to competing approaches is that to evaluate information gain over the map, only a very coarse prior map estimate needs to be computed. Its coarseness is independent and does not jeopardize the Pose SLAM estimate. Moreover, a replanning scheme is devised to detect significant localization improvement during path execution. The approach is tested in simulations in a common publicly available dataset comparing favorably against frontier based exploration.

Ensuring that a particular and unsuspecting member of a group is the recipient of a salient-item hand-over is a complicated interaction. The robot must effectively, expediently and reliably communicate its intentions to advert any tendency within the group towards antinormative behaviour. In this paper, we study how a robot can establish the participant roles of such an interaction using imitated social and contextual cues. We designed two gaze cues, the first was designed to discourage antinormative behaviour through individualising a particular member of the group and the other to the contrary. We designed and conducted a field experiment (456 participants in 64 trials) in which small groups of people (between 3 and 20 people) assembled in front of the robot, which then attempted to pass a salient object to a particular group member by presenting a physical cue, followed by one of two variations of a gaze cue. Our results showed that presenting the individualising cue had a significant (z=3.733, p=0.0002) effect on the robot's ability to ensure that an arbitrary group member did not take the salient object and that the selected participant did.

Detection of individuals' intention from a sequence of actions is an open and complex problem. In this paper we present a smart walker as mobility aid which can interpret the users' behaviour patterns to recognize their intentions and consequently act as an intelligent assistant. The result of the experiments performed in this paper demonstrates the potential of dynamic bayesian networks (DBN), in relation to their dynamic and unsupervised nature, for realistic human-robot interaction modelling.

This paper formulates the SLAM problem using 2D laser data as an optimization problem. The environment is modeled as a set of curves and the variables of the optimization problem are the robot poses as well as the parameters describing the curves. There are two key differences between this SLAM formulation and existing SLAM methods. First, the environment is represented by continuous curves instead of point clouds or occupancy grids. Second, all the laser readings, including laser beams which returns its maximum range value, have been included in the objective function. As the objective function to be optimized contains discontinuities, it can not be solved by standard gradient based approaches and thus a Genetic Algorithm (GA) based method is applied. Matching of laser scans acquired from relatively far apart robot poses is achieved by applying GA on top of the Iterative closest point (ICP) algorithm. The new SLAM formulation and the use of a global optimization algorithm successfully avoid the convergence to local minimum for both the scan matching and the SLAM problem. Both simulated and experimental data are used to demonstrate the effectiveness of the proposed techniques.

This paper presents a framework for modelling the interaction between a human operator and a robotic device, that enables the robot to collaborate with the human to jointly accomplish tasks. States of the system are captured in a model based on a partially observable Markov decision process (POMDP). States representing the human operator are motivated by behaviours from the psychology of the human action cycle. Hierarchical nature of these states allows the exploitation of data structures based on algebraic decision diagrams (ADD) to efficiently solve the resulting POMDP. The proposed framework is illustrated using two examples from assistive robotics; a robotic wheel chair and an intelligent walking device. Experimental results from trials conducted in an office environment with the wheelchair is used to demonstrate the proposed technique.

This paper presents a new unified feature parametrization approach for monocular SLAM. The parametrization is based on the parallax angle and can reliably represent both nearby and distant features, as well as features in the direction of camera motion and features observed only once. A new bundle adjustment (BA) algorithm using the proposed parallax angle parametrization is developed and shown to be more reliable as compared with existing BA algorithms that use Euclidean XYZ or inverse depth parametrizations. A new map joining algorithm that allows combining a sequence of local maps generated using BA with the proposed parametrization, that avoids the large computational cost of a global BA, and can automatically optimize the relative scales of the local maps without any loss of information, is also presented. Extensive simulations and a publicly available large-scale real dataset with centimeter accuracy ground truth are used to demonstrate the accuracy and consistency of the BA and map joining algorithms using the new parametrization. Especially, since the relative scales are optimized automatically in the proposed BA and map joining algorithms, there is no need to compute any relative scales even for a loop more than 1km.

In this paper, an optimisation model based on Pickup and Delivery Problem with Time Windows (PDPTW), and an exact algorithm based on Branch-and-Bound with Column Generation (BBCG), are presented for Autonomous Straddle Carriers Scheduling (ASCS) problem at automated container terminals. The ASCS problem is firstly modeled into a PDPTW, which is formulated as a Binary Integer Programming (BIP) and then solved by Column Generation (CG) in the Branch-and-Bound (BB) framework. The BBCG algorithm is also compared to another two exact algorithms [i.e., Binary integer Programming with Dynamic Programming (BPDP) and Exhaustive Search with Permutation and Combination (ESPC)] for the ASCS problem solving. Based on the map of an actual automated container terminal, simulation results and discussions are presented to demonstrate the effectiveness and efficiency of the presented model and algorithm for autonomous vehicle scheduling.

One of the leading causes of automobile accidents is related to driving impairment due to drowsiness. A large percentage of these accidents occur due to drivers' unawareness of the degree of impairment. An automatic detection of drowsiness levels could lead to lower accidents and hence lower fatalities. However, the significant fluctuations of the drowsiness state within a short time poses a major challenge in this problem. In response to such a challenge, we present the Uncorrelated Fuzzy Locality Preserving Analysis (UFLPA) feature projection method. The proposed UFLPA utilizes the changes in driver behavior, by means of the corresponding Electroencephalogram (EEG), Electrooculogram (EOG), and Electrocardiogram (ECG) signals to extract a set of features that can highly discriminate between the different drowsiness levels. Unlike existing methods, the proposed UFLPA takes into consideration the fuzzy nature of the input measurements while preserving the local discriminant and manifold structures of the data. Additionally, UFLPA also utilizes Singular Value Decomposition (SVD) to avoid the singularity problem and produce a set of uncorrelated features. Experiments were performed on datasets collected from thirty-one subjects participating in a simulation driving test with practical results indicating the significance of the results achieved by UFLPA of 94%-95% accuracy on average across all subjects.

An efficient automatic solution to the feature-based simultaneous localisation and mapping (SLAM) of mobile robots operating in conditions where a number of competing objectives operate simultaneously is proposed. The formulation quantitatively measures the merit of incoming data with respect to multiple priorities, automatically adjusting the amount of observations to be used in the estimation process for the best possible combined outcome. The methodology enables a selection mechanism which can efficiently exploit the observations available to the robot to best fulfil the objectives of differing tasks throughout the course of a mission, e.g. localisation, mapping, exploration, feature distribution, searching for specific objects or victims, etc. The work is particularly motivated by navigation in three-dimensional terrains, and an example considering the objectives of robot localisation and map expansion in a search and rescue environment using an RGB-D camera is utilised for discussion and results.

Robotic devices are increasingly penetrating the human work spaces as stand alone units and helpers. It is believed that a robot could be easily integrated with humans, if the robot can learn how to behave in a socially acceptable manner. This involves a robot to observe, learn and comply with basic rules of human behaviors. As an example, one would expect a robot to travel in an environment without intruding human workspaces unnecessarily. Thus, identifying common motion patterns of people by observing a specific environment is an important task as people's trajectories are usually not random, however are tailored to the way the environment is structured. We propose a learning algorithm to construct a Sampled Hidden Markov Model (SHMM) that captures behavior of people through observations and then demonstrate how this model could be exploited for planning socially aware paths. Experimental results are presented to demonstrate the viability of the proposed approach.

Human robot interaction is an emerging area of research, where human understandable robotic representations can play a major role. Knowledge of semantic labels of places can be used to effectively communicate with people and to develop efficient navigation solutions in complex environments. In this paper, we propose a new approach that enables a robot to learn and classify observations in an indoor environment using a labeled semantic grid map, which is similar to an Occupancy Grid like representation. Classification of the places based on data collected by laser range finder (LRF) is achieved through a machine learning approach, which implements logistic regression as a multi-class classifier. The classifier output is probabilistically fused using independent opinion pool strategy. Appealing experimental results are presented based on a data set gathered in various indoor scenarios.

This paper addresses the problem of mapping three dimensional environments from a sequence of images taken by a calibrated camera, and simultaneously generating the camera motion trajectory. This is the Monocular SLAM problem in robotics, and is akin to the Structure from Motion (SFM) problem in computer vision. We present a novel map-aided 6-DOF relative pose estimation method based on a new formulation of the Monocular SLAM that is able to provide better initial estimates of new camera poses than the simple triangulation traditionally used in this context. The '6-DOF' means relative to the map which itself is up to an unobservable scale. The proposed pose estimator also allows more effective outlier rejection in matching features present in the map and features extracted from two consecutive images. Our Monocular SLAM algorithm is able to deal with arbitrary camera motion, making the smooth motion assumption, which is required by the typically used constant velocity model, unnecessary. In the new Monocular SLAM formulation, the measurements of extracted features from images are partitioned into those used for the estimation of the environment and those used for estimating the camera motion. The new formulation enables the current map estimate to aid achieving the full 6-DOF relative pose estimation up to the mapping scale while maximally exploiting the geometry information in images. Experiment results are provided to verify the proposed algorithm.

This paper first demonstrates an interesting property of bundle adjustment (BA), “scale drift correction”. Here “scale drift correction” means that BA can converge to the correct solution (up to a scale) even if the initial values of the camera pose translations and point feature positions are calculated using very different scale factors. This property together with other properties of BA makes it the best approach for monocular Simultaneous Localization and Mapping (SLAM), without considering the computational complexity. This naturally leads to the idea of using local BA and map joining to solve large-scale monocular SLAM problem, which is proposed in this paper. The local maps are built through Scale-Invariant Feature Transform (SIFT) for feature detection and matching, random sample consensus (RANSAC) paradigm at different levels for robust outlier removal, and BA for optimization. To reduce the computational cost of the large-scale map building, the features in each local map are judiciously selected and then the local maps are combined using a recently developed 3D map joining algorithm. The proposed large-scale monocular SLAM algorithm is evaluated using a publicly available dataset with centimeter-level ground truth.

The main contribution of this paper is a mathematical model describing performance metrics for coordinating multiple mobile robots in a seaport container terminal. The scenario described here requires dealing with many difficult practical challenges such as the presence of multiple levels of container stacking and sequencing, variable container orientations, and vehicular dynamics that require finite acceleration and deceleration times. Furthermore, in contrast to the automatically guided vehicle planning problem in a manufacturing environment, the container carriers described here are free ranging. Although, the port structure imposes a set of “virtual” roadways along which the vehicles are allowed to travel, path planning is essential in preventing contention and collisions. A performance metric which minimises total yard-vehicle usage, while producing robust traffic plans by encouraging both early starting and finishing of jobs is presented for different vehicle fleet sizes and job allocation scenarios.

In many mobile robotic manipulation tasks it is desirable to interact with the robots surroundings without actually grasping the object being manipulated. Non-prehensile manipulation allows a robot to interact in situations which would otherwise be impossible due to size or weight. This paper presents the derivation of a mathematical model of an object pushed by a single point and sliding in the presence of friction where the dynamic effects of mass and inertia are significant. This model is validated using numerical simulation. The derived dynamic model is also compared with a kinematic approximation from literature, showing that under certain conditions, the motion of a pushed object is similar to the motion of a non-holonomic vehicle. Finally, the results of experimental investigations are discussed and promising directions for further work are proposed.

In recent SLAM (simultaneous localization and mapping) literature, Pose Only optimization methods have become increasingly popular. This is greatly supported by the fact that these algorithms are computationally more efficient, as they focus more on the robots trajectory rather than dealing with a complex map. Implementation simplicity allows these to handle both 2D and 3D environments with ease. This paper presents a detailed evaluation on the reliability and accuracy of Pose Only SLAM, and aims at providing a definitive answer to whether optimizing poses is more advantages than optimizing features. Focus is centered around TORO, a Tree based network optimization algorithm, which has gained increased recognition within the robotics community. We compare this with Least Squares, which is often considered one of the best Maximum Likelihood method available. Results are based on both simulated and real 2D environments, and presented in a way where our conclusions can be substantiated.

Most people believe SLAM is a complex nonlinear estimation/optimization problem. However, recent research shows that some simple iterative methods based on linearization can sometimes provide surprisingly good solutions to SLAM without being trapped into a local minimum. This demonstrates that hidden structure exists in the SLAM problem that is yet to be understood. In this paper, we first analyze how far SLAM is from a convex optimization problem. Then we show that by properly choosing the state vector, SLAM problem can be formulated as a nonlinear least squares problem with many quadratic terms in the objective function, thus it is clearer how far SLAM is from a linear least squares problem. Furthermore, we explain that how the map joining approaches reduce the nonlinearity/nonconvexity of the SLAM problem.

This paper presents a statistically consistent SLAM algorithm where the environment is represented using a collection of B-Splines. The use of B-Splines allow environment to be represented without having to extract specific geometric features such as lines or points. Our previous work proposed a new observation model that enables raw measurements taken from a laser range finder to be transferred into relative position information between the control points of a B-Spline and the robot pose where the observation is made. One of the unresolved issues in the work was the estimation of the observation covariance, which is addressed through an analytical approach in this paper. As the uncertainty associated with the observation model is accurately defined and an optimization approach is used in the estimation process, the proposed SLAM algorithm can produce consistent estimates. Both simulation and experimental data are used for evaluation of the results.

A generic methodology to plan increasingly stable paths for mobile platforms travelling over uneven terrain is proposed in this paper. This is accomplished by extending the Fast Marching level-set method of propagating interfaces in 3D lattices with an analytical kyno-dynamic metric which embodies robot stability in the given terrain. This is particularly relevant for reconfigurable platforms which significantly modify their mass distribution through posture adaptation, such as robots equipped with manipulator arms or varying traction arrangements. Results obtained from applying the proposed strategy in a mobile rescue robot operating on simulated and real terrain data illustrate the validity of the proposed strategy.

Human robot interaction is an emerging area of research with many challenges. Knowledge about human behaviors could lead to more effective and efficient interactions of a robot in populated environments. This paper presents a probabilistic framework for the learning and representation of human motion patterns in an office environment. It is based on the observation that most human trajectories are not random. Instead people plan trajectories based on many considerations, such as social rules and path length. Motion patterns are learned using an incrementally growing Sampled Hidden Markov Model. This model has a number of interesting properties which can be of use in many applications. For example, the learned knowledge can be used to predict motion, infer social rules, thus improve a robot's operation and its interaction with people in a populated space. The proposed learning method is extensively validated in real world experiments.

Extending metric space representations of an environment with other high level information, such as semantic and topological representations enable a robotic device to efficiently operate in complex environments. This paper proposes a methodology for a robot to classify indoor environments into semantic categories. Classification task, using data collected from a laser range finder, is achieved by a machine learning approach based on the logistic regression algorithm. The classification is followed by a probabilistic temporal update of the semantic labels of places. The innovation here is that the new algorithm is able to classify parts of a single laser scan into different semantic labels rather than the conventional approach of gross categorization of locations based on the whole laser scan. We demonstrate the effectiveness of the algorithm using a data set available in the public domain.

Relating information originating from disparate sensors observing a given scene is a challenging task, particularly when an appropriate model of the environment or the behaviour of any particular object within it is not available. One possible strategy to address this task is to examine whether the sensor outputs contain information which can be attributed to a common cause. In this paper, we present an approach to localise this embedded common information through an indirect method of estimating mutual information between all signal sources. Ability of L1regularization to enforce sparseness of the solution is exploited to identify a subset of signals that are related to each other, from among a large number of sensor outputs. As opposed to the conventional L2regularization, the proposed method leads to faster convergence with much reduced spurious associations. Simulation and experimental results are presented to validate the findings.

This paper describes a simultaneous localization and mapping (SLAM) algorithm for use in unstructured environments that is effective regardless of the geometric complexity of the environment. Features are described using B-splines as modeling tool, and the set of control points defining their shape is used to form a complete and compact description of the environment, thus making it feasible to use an extended Kalman-filter (EKF) based SLAM algorithm. This method is the first known EKF-SLAM implementation capable of describing general free-form features in a parametric manner. Efficient strategies for computing the relevant Jacobians, perform data association, initialization, and map enlargement are presented. The algorithms are evaluated for accuracy and consistency using computer simulations, and for effectiveness using experimental data gathered from different real environments.

This paper presents a new technique for evaluating the observability of the simultaneous localization and mapping (SLAM) problem. The state vector of an estimation theoretic formulation of the SLAM problem is recast to include all robot poses from which the measurements are made. This converts SLAM to a problem of estimating a set of unknown, constant random variables. Fisher Information Matrix of the resulting static estimation problem is derived and analyzed to examine the observability of SLAM. Outcomes of the analysis and comparisons to the observability analysis presented in recent literature are presented. Proposed technique makes it possible to analyze the observability of a range of SLAM problems with ease.

The main contribution of this paper arises from the development of a new framework, which has its inspiration in the mechanics of human navigation, for solving the problem of Simultaneous Localization and Mapping (SLAM). The proposed framework has specific relevance to vision based SLAM, in particular, small baseline stereo vision based SLAM and addresses several key issues relevant to the particular sensor domain. Firstly, as observed in the authors' earlier work, the particular sensing device has a highly nonlinear observation model resulting in inconsistent state estimations when standard recursive estimators such as the Extended Kalman Filter (EKF) or the Unscented variants are used. Secondly, vision based approaches tend to have issues related to large feature density, narrow field of view and the potential requirement of maintaining large databases for vision based data association techniques. The proposed Multi Map SLAM solution addresses the filter inconsistency issue by formulating the SLAM problem as a nonlinear batch optimization. Feature management is addressed through a two tier map representation. The two maps have unique attributes assigned to them. The Global Map (GM) is a compact global representation of the robots environment and the Local Map (LM) is exclusively used for low-level navigation between local points in the robot's navigation horizon.

This paper provides a novel state vector and covariance sub-matrix recovery algorithm for a recently developed submap based exactly sparse extended information filter (EIF) SLAM algorithm - sparse local submap joining filter (SLSJF). The algorithm achieves exact recovery instead of approximate recovery. The recovery algorithm is very efficient because of an incremental Cholesky factorization approach and a natural reordering of the global state vector. Simulation results show that the computation cost of the SLSJF is much lower as compared with the sequential map joining algorithm using extended Kalman filter (EKF). The SLSJF with the proposed recovery algorithm is also successfully applied to the Victoria Park data set.

This paper considers the trajectory planning problem for line-feature based SLAM in structured indoor environments. The robot poses and line features are estimated using Smooth and Mapping (SAM) which is found to provide more consistent estimates than the Extended Kalman Filter (EKF). The objective of trajectory planning is to minimise the uncertainty of the estimates and to maximise coverage. Trajectory planning is performed using Model Predictive Control (MPC) with an attractor incorporating long term goals. This planning is demonstrated both in simulation and in a real-time experiment with a Pioneer2DX robot.

This paper presents an intelligent decision-making agent to assist wheelchair users in their daily navigation activities. Several navigational techniques have been successfully developed in the past to assist with specific behaviours such as "door passing" or "corridor following". These shared control strategies normally require the user to manually select the level of assistance required during use. Recent research has seen a move towards more intelligent systems that focus on forecasting users' intentions based on current and past actions. However, these predictions have been typically limited to locations immediately surrounding the wheelchair. The key contribution of the work presented here is the ability to predict the users' intended destination at a larger scale, that of a typical office arena. The systems relies on minimal user input - obtained from a standard wheelchair joystick - in conjunction with a learned Partially Observable Markov Decision Process (POMDP), to estimate and subsequently drive the user to his destination. The projection is constantly being updated, allowing for true user- platform integration. This shifts users' focus from fine motor- skilled control to coarse control broadly intended to convey intention. Successful simulation and experimental results on a real wheelchair robot demonstrate the validity of the approach.

This paper presents a novel solution for building three-dimensional dense maps in unknown and unstructured environment with reduced computational costs. This is achieved by giving the robot the ‘intelligence’ to select, out of the steadily collected data, the maximally informative observations to be used in the estimation of the robot location and its surroundings. We show that, although the actual evaluation of information gain for each frame introduces an additional computational cost, the overall efficiency is significantly increased by keeping the matrix compact. The noticeable advantage of this strategy is that the continuously gathered data is not heuristically segmented prior to be input to the filter. Quite the opposite, the scheme lends itself to be statistically optimal and is capable of handling large data sets collected at realistic sampling rates. The strategy is generic to any 3D feature-based simultaneous localization and mapping (SLAM) algorithm in the information form, but in the work presented here it is closely coupled to a proposed novel appearance-based sensory package. It consists of a conventional camera and a range imager, which provide range, bearing and elevation inputs to visual salient features as commonly used by three-dimensional point-based SLAM, but it is also particularly well adapted for lightweight mobile platforms such as those commonly employed for Urban Search and Rescue (USAR), chosen here to demonstrate the excellences of the proposed strategy.

This paper presents a novel local submap joining algorithm for building large-scale feature-based maps: sparse local submap joining filter (SLSJF). The input to the filter is a sequence of local submaps. Each local submap is represented in a coordinate frame defined by the robot pose at which the map is initiated. The local submap state vector consists of the positions of all the local features and the final robot pose within the submap. The output of the filter is a global map containing the global positions of all the features as well as all the robot start/end poses of the local submaps. Use of an extended information filter (EIF) for fusing submaps makes the information matrix associated with SLSJF exactly sparse. The sparse structure together with a novel state vector and covariance submatrix recovery technique makes the SLSJF computationally very efficient. The SLSJF is a canonical and efficient submap joining solution for large-scale simultaneous localization and mapping (SLAM) problems that makes use of consistent local submaps generated by any reliable SLAM algorithm. The effectiveness and efficiency of the new algorithm is verified through computer simulations and experiments.

This paper presents a novel vision-based sensory package and an information-efficient simultaneous localization and mapping (SLAM) algorithm. Together, we offer a solution for building 3-D dense map in an unknown and unstructured environment with minimal computational costs. The sensory package we adopt consists of a conventional camera and a range imager, which provide range and bearing and elevation inputs as commonly used by 3-D feature-based SLAM. In addition, we propose an algorithm to give the robots the `intelligencerdquo to select, out of the steadily collected data, the maximally informative observations to be used in the estimation process. We show that, although the actual evaluation of information gain for each frame introduces an additional computational cost, the overall efficiency is significantly increased by keeping the matrix compact. The noticeable advantage of this strategy is that the continuously gathered data are not heuristically segmented prior to being input to the filter. Quite the opposite, the scheme lends itself to be statistically optimal and is capable of handling large datasets collected at realistic sampling rates.

Providing safety critical information to the driver is vital in reducing road accidents, especially at intersections. Intersections are complex to deal with due to the presence of large number of vehicle and pedestrian activities, and possible occlusions. Information available from only the sensors onboard a vehicle has limited value in this scenario. In this paper, we propose to utilize sensors on-board the vehicle of interest as well as the sensors that are mounted on nearby vehicles to enhance the driver situation awareness. The resulting major research challenge of sensor registration with moving observers is solved using a mutual information based technique. The response of the sensors to common causes are identified and exploited for computing their unknown relative locations. Experimental results, for a mock up traffic intersection in which mobile robots equipped with laser range finders are used, are presented to demonstrate the efficacy of the proposed technique.

A problem of searching with multiple searchers and scouts is presented. Unlike most search problems that terminate as soon as the target is found, successful detection by scouts only improve on the current knowledge of the moving target's location, such that the searchers can more effectively find and service the target in the future. The team must correspondingly plan not only to maximize the probability of the searchers directly finding the target, but also give them the best chance of exploiting any new information from potential scout detections. It is shown that this need to plan for replanning can be addressed by equivalently solving a series of simpler detection search problems that always do terminate on detection. Optimal and heuristic solution methods for this searcher/scout problem are derived, such that the capabilities of all the sensing platforms in a search task are harnessed even when only a subset are capable of actually servicing the target.

Most of the lane marking detection algorithms reported in the literature are suitable for highway scenarios. This paper presents a novel clustered particle filter based approach to lane detection, which is suitable for urban streets in normal traffic conditions. Furthermore, a quality measure for the detection is calculated as a measure of reliability. The core of this approach is the usage of weak models, i.e. the avoidance of strong assumptions about the road geometry. Experiments were carried out in Sydney urban areas with a vehicle mounted laser range scanner and a ccd camera. Through experimentations, we have shown that a clustered particle filter can be used to efficiently extract lane markings.

The F2(Force Field) method is a novel approach for multi-robot motion planning and collision avoidance. The setting of parameters is however vital to its performance. This paper presents an approach using Particle Swarm Optimization (PSO) to properly determine the control parameters for the F2method. The goal of the optimization is to minimize the resultant path lengths. The approach presented in this paper can be used as a tool to obtain optimal parameters for various tasks before their execution. Simulations are carried out in various environments to show the feasibility of this approach.

This paper investigates the convergence properties and consistency of extended Kalman filter (EKF) based simultaneous localization and mapping (SLAM) algorithms. Proofs of convergence are provided for the nonlinear two-dimensional SLAM problem with point landmarks observed using a range-and-bearing sensor. It is shown that the robot orientation uncertainty at the instant when landmarks are first observed has a significant effect on the limit and/or the lower bound of the uncertainties of the landmark position estimates. This paper also provides some insights to the inconsistencies of EKF based SLAM that have been recently observed. The fundamental cause of EKF SLAM inconsistency for two basic scenarios are clearly stated and associated theoretical proofs are provided.

In the past few years, there has been significant advancement in localization and mapping using stereo cameras. Despite the recent successes, reliably generating an accurate geometric map of a large indoor area using stereo vision still poses significant challenges due to the accuracy and reliability of depth information especially with small baselines. Most stereo vision based applications presented to date have used medium to large baseline stereo cameras with Gaussian error models. Here we make an attempt to analyze the significance of errors in small baseline (usually <0.1m) stereo cameras and the validity of the Gaussian assumption used in the implementation of Kalman filter based SLAM algorithms. Sensor errors are analyzed through experimentations carried out in the form of a robotic mapping. Then we show that SLAM solutions based on the extended Kalman filter (EKF) could become inconsistent due to the nature of the observation models used

Multimodal sensor registration and calibration are crucially important aspects in distributed sensor fusion. Unknown relationships of sensors and joint probability distribution between sensory signals make the sensor fusion nontrivial. In this paper, we adopt a Mutual Information (MI) based approach for sensor registration and calibration. It is based on unsupervised learning of a nonparametric sensing model by maximizing mutual information between signal streams. Experiments were carried out in an office like environment with two laser sensors capturing arbitrarily moving people. Attributes of the moving targets are used. Problems due to target occlusions are alleviated by the multiple model tracker. The registration and calibration methodology does not require any artificially generated patterns or motions unlike other calibration methodologies

The main contribution of this paper is a theoretical analysis of the extended Kalman filter (EKF) based solution to the simultaneous localisation and mapping (SLAM) problem. The convergence properties for the general nonlinear two-dimensional SLAM are provided. The proofs clearly show that the robot orientation error has a significant effect on the limit and/or the lower bound of the uncertainty of the landmark location estimates. Furthermore, some insights to the performance of EKF SLAM and a theoretical analysis on the inconsistencies in EKF SLAM that have been recently observed are given

The main contribution of this paper is a new SLAM algorithm for the mapping of large scale environments by combining local maps. The local maps can be generated by traditional extended Kalman filter (EKF) based SLAM. Relationships between the locations of the landmarks in the local map are then extracted and used in an extended information filter (EIF) to build a global map. An important feature is that the information matrix for the global map is exactly sparse, leading to significant computational advantages. This paper thus presents an algorithm that combines the advantages of both the existing local map joining SLAM algorithms, which reduces the linearization error in EKF SLAM and allows computationally demanding global map fusion to be scheduled off-line, and the decoupled SLAM (D-SLAM) algorithm, which provides an efficient strategy for building large maps using relative location information. The effectiveness of the new algorithm is illustrated through computer simulations.

Knowledge of calibration, that defines the location of sensors relative to each other, and registration, that relates sensor response due to the same physical phenomena, are essential in order to be able to fuse information from multiple sensors. In this paper, a mutual information (MI) based approach for automatic sensor registration and calibration is presented. Unsupervised learning of a nonparametric sensing model by maximizing mutual information between signal streams is used to relate information from different sensors, allowing unknown sensor registration and calibration to be determined. Experiments conducted in an office environment are used to illustrate the effectiveness of the proposed technique. Two laser sensors are used to capture people mobbing in an arbitrarily manner in the environment and MI from a number of attributes of the motion are used for relating the signal streams from the sensors. Thus the sensor registration and calibration is achieved without using artificial patterns or pre-specified motions

With limited dynamic range and poor noise performance, cameras still pose considerable challenges in the application of range sensors in the context of robotic navigation, especially in the implementation of simultaneous localisation and mapping (SLAM) with sparse features. This paper presents a combination of methods in solving the SLAM problem in a constricted indoor environment using small baseline stereo vision. Main contributions include a feature selection and tracking algorithm, a stereo noise filter, a robust feature validation algorithm and a multiple hypotheses adaptive window positioning method in 'closing the loop'. These methods take a novel approach in that information from the image processing and robotic navigation domains are used in tandem to augment each other. Experimental results including a real-time implementation in an office-like environment are also presented

We consider a search for a target moving within a known indoor environment partitioned into interconnected regions of varying sizes. The knowledge of target location is described as a probability distribution over the regions, and the searcher can only move from one region to another as the structure allows. The objective is to find a feasible path through the regions that maximizes the probability of locating the target within fixed time. This problem generalizes the existing optimal searcher path problem (OSP) by additionally stipulating a minimum amount of time that a finite-speed searcher must spend to travel through a region before reaching the next. We propose a technique to obtain the upper bound of detection for solving the problem in a branch and bound framework. Comparisons show that the technique is also superior to known bounding methods for the original optimal searcher path problem

Active SLAM poses the challenge for an autonomous robot to plan efficient paths simultaneous to the SLAM process. The uncertainties of the robot, map and sensor measurements, and the dynamic and motion constraints need to be considered in the planning process. In this paper, the active SLAM problem is formulated as an optimal trajectory planning problem. A novel technique is introduced that utilises an attractor combined with local planning strategies such as model predictive control (a.k.a. receding horizon) to solve this problem. An attractor provides high level task intentions and incorporates global information about the environment for the local planner, thereby eliminating the need for costly global planning with longer horizons. It is demonstrated that trajectory planning with an attractor results in improved performance over systems that have local planning alone

The main contribution of this paper is a novel stereo-based algorithm which serves as a tool to examine the viability of stereo vision solutions to the simultaneous localisation and mapping (SLAM) for large indoor environments. Using features extracted from the scale invariant feature transform (SIFT) and depth maps from a small vision system (SVS) stereo head, an extended Kalman filter (EKF) based SLAM algorithm, that allows the independent use of information relating to depth and bearing, is developed. By means of a map pruning strategy for managing the computational cost, it is demonstrated that statistically consistent location estimates can be generated for a small (6 m times 6 m) structured office environment, and in a robotics search and rescue arena of similar size. It is shown that in a larger office environment, the proposed algorithm generates location estimates which are topologically correct, but statistically inconsistent. A discussion on the possible reasons for the inconsistency is presented. The paper highlights that, despite recent advances, building accurate geometric maps of large environments with vision only sensing is still a challenging task

A novel force field (F2) method with variable speed for multi-robot motion planning and collaboration is presented in this paper. The basic concept of the F2method is to generate a force field for every robot based on and continuously changing according to its status including traveling speed, dimension, priority, location and environmental factor, etc. The interactions among robots' force fields and obstacles provide a natural way for collision avoidance and collaboration while robots are on their way to goals. Previous F2method assumes that robots travel with constant speeds and can react instantly to the resultant force to change their orientations. Starting from a problematic situation brought out by this hypothesis, this paper remedies the F2method by taking robots' dynamics and kinematics characteristics into consideration. In the variable speed force field method (VSF2), a robot can change its own speed according to environment information and its own status. Simulations in a real indoor environment were carried out and demonstrated the feasibility and effectiveness of this method

In this paper, the possibility and necessity of multi-step trajectory planning in Extended Kalman Filter (EKF) based SLAM is investigated. The objective of the trajectory planning here is to minimize the estimation error of the robot and landmark locations subject to a given time horizon. We show that the problem can be regarded as an optimization problem for a gradually identified model. A numerical method is proposed for trajectory planning using a variant of the nonlinear Model Predictive Control (MPC). The proposed method is optimal in the sense that the control action is computed using all the information available at the time of decision making. Simulation results are included to compare the results from the one-step look-ahead trajectory planning and the proposed multi-step look-ahead technique.

Use of a Gaussian Sum filter (GSF) to efficiently solve the initialisation problem in bearing-only simultaneous localisation and mapping (SLAM) is the main contribution of this paper. When information about the range is not available, the initial probability density function (pdf) of a landmark in the environment can not be represented using a Gaussian. The GSF is an attractive candidate for estimation in this scenario as it can deal with arbitrary pdfs represented as sets of Gaussians. However, the implementation of the GSF requires maintaining a bank of extended Kalman filters. The resulting computational complexity needs to be reduced by employing a minimum number of filters. In this work, the performance of each extended Kalman filter (EKF) in the GSF is evaluated using the sequential probability ratio test (SPRT). As such the number of members in the Gaussian sum can be reduced rapidly and the efficiency of the GSF can be significantly increased, providing a solution to the important problem of bearing-only SLAM. The effectiveness of the proposed approach is demonstrated by simulation and experiment conducted using a Pioneer mobile robot.

The main contribution of this paper is an algorithm for integrating motion planning and simultaneous localisation and mapping (SLAM). Accuracy of the maps and the robot locations computed using SLAM is strongly dependent on the characteristics of the environment, for example feature density, as well as the speed and direction of motion of the robot. Appropriate control of the robot motion is particularly important in bearing-only SLAM, where the information from a moving sensor is essential. In this paper a near minimum time path planning algorithm with a finite planning horizon is proposed for bearing-only SLAM. The objective of the algorithm is to achieve a predefined mapping precision while maintaining acceptable vehicle location uncertainty in the minimum time. Simulation results have shown the effectiveness of the proposed method.

The main contribution of this paper is an algorithm for autonomous search that minimizes the expected time for detecting multiple targets present in a known built environment. The proposed technique makes use of the probability distribution of the target(s) in the environment, thereby making it feasible to incorporate any additional information, known a-priori or acquired while the search is taking place, into the search strategy. The environment is divided into a set of distinct regions and an adjacency matrix is used to describe the connections between them. The costs of searching any of the regions as well as the cost of travel between them can be arbitrarily specified. The search strategy is derived using a dynamic programming algorithm. The effectiveness of the algorithm is illustrated using an example based on the search of an office environment. An analysis of the computational complexity is also presented.

This paper provides a solution to the optimal trajectory planning problem in target localisation for multiple heterogeneous robots with bearing-only sensors. The objective here is to find robot trajectories that maximise the accuracy of the locations of the targets at a prescribed terminal time. The trajectory planning is formulated as an optimal control problem for a nonlinear system with a gradually identified model and then solved using nonlinear model predictive control (MPC). The solution to the MPC optimisation problem is computed through exhaustive expansion tree search (EETS) plus sequential quadratic programming (SQP). Simulations were conducted using the proposed methods. Results show that EETS alone performs considerably faster than EETS+SQP with only minor differences in information gain, and that a centralised approach outperforms a decentralised one in terms of information gain. We show that a centralised EETS provides a near optimal solution. We also demonstrate the significance of using a matrix to represent the information gathered.

In this paper, we propose a new algorithm for SLAM that makes use of a state vector consisting of quantities that describe the relative locations among features. In contrast to previous relative map strategies, the new state vector is compact and always consists of 2n - 3 elements (in a 2D environment) where n is the number of features in the map. It is also shown that the information from observations can be transformed and grouped into two parts: first one containing the information about the map and the second one containing the information about the robot location relative to the features in the map. Therefore the SLAM can be decoupled into two processes where mapping uses the first part of the transformed observation vector and localization becomes a 3-dimensional estimation problem. It is also shown that the information matrix of the map is exactly sparse, resulting in potential computational savings when an information filter is used for mapping. The new decoupled SLAM algorithm is called D-SLAM and is illustrated using simulation.

This paper presents the real-time control of cooperative unmanned air vehicles (UAV) that dynamically engage multiple targets in a time-optimal manner. Techniques to dynamically allocate vehicles to targets and to subsequently find the time-optimal control actions are proposed. The decentralization of the proposed control strategy is further presented such that the vehicles can be controlled in real-time without significant time delay. The proposed strategy is men applied to various practical battlefield problems, and numerical results show the efficiency of the proposed strategy.

This paper presents a control strategy for the pursuer in the pursuit-evasion game problem when the evader behaves intelligently. The pursuer in the proposed technique does not try to react to the evader's behavior instantaneously. The proposed technique therefore does not yield instantaneous optimality but capture the evader in a time-efficient and robust fashion even when the evader is intelligent. The proposed technique was applied to two numerical examples and the results were compared to those by the conventional motion tracking algorithms. The results and comparison show that the proposed technique could capture the evader faster than the conventional motion tracking algorithms in both the examples.

This paper provides a technique for minimal time robot motion control in the estimation-theoretic based simultaneous localizations and map building (SLAM) problem. We consider the scenario that the robot needs to go to a destination which is a prescribed location in the coordinate system referenced by its starting position. The task of the robot is to reach the destination within minimal time while localizing itself and building a map of the environment with a prescribed accuracy. This task may be a real navigation task or may be a subtask in a SLAM problem of a large unknown environment. A global sub-optimal control law is derived using dynamic programming techniques.

This paper presents a multiple hypothesis approach to solve the simultaneous localisation and mapping (SLAM) problem with a bearing-only sensor. The main contribution of the paper is to provide a remedy for the landmark initialisation problem that occurs due to the absence of range information, in a computationally efficient manner. Each landmark is initialised in the form of multiple hypotheses distributed along the direction of the bearing measurement. Using subsequent measurements, the validity of the hypotheses is evaluated based on the sequential probability ratio test (SPRT). Consequently, the best approximation to the landmark location is maintained. This approach enables an extended Kalman filler (EKF) to be used for bearing-only SLAM providing a computational efficient solution. Simulation and experimental results, from using a camera as the bearing-only sensor mounted on a Pioneer robot are included to demonstrate the effectiveness of the proposed technique.

This paper proposes a method for simultaneous localisation and mapping (SLAM) in an indoor environment using stereo vision. Specially designed artificial landmarks distributed in the environment are observed and extracted from a camera image. The disparity map obtained from the stereo vision system is used to obtain the ranges to these landmarks. The main contribution of the paper is the formulation of the mathematical framework for SLAM for a robot moving on a planar surface among landmarks distributed in three dimensional space. The paper also presents the results of experiments conducted using a pioneer robot and a Triclops stereo vision system. It is demonstrated that accurate robot and feature locations can be obtained using the proposed technique.

Speech activated interface between human and autonomous/semi-autonomous systems requires accurate voice detection and recognition. In this area, pitch and end-point detection is of vital importance. This paper presents a new method for pitch detection based on the phase of the continuous wavelet transform. The advantage of the proposed technique is that it can serve not only as an accurate pitch detector, but also can offer an efficient solution to the end-point detection problem. Experimental results are provided for the detection of pitch periods and end points in a neural-network based voice enabled wheelchair system.

This paper presents a formulation and solution of the time-optimal control of multiple cooperative robot vehicles. In particular, a group of robot vehicles reaching a terminal state in absolute and/or relative formations in minimum time is addressed. A canonical formulation of the problem is first derived and a numerical technique, which can effectively solve this class of problems, is then proposed. Numerical results are then presented to demonstrate the efficacy of the proposed formulation and method of solution. The techniques described offer a practical solution to the problem of building and controlling formations of cooperative autonomous vehicles in real-time.

This paper presents a solution to the real-time control of cooperative unmanned air vehicles (UAVs) that engage multiple targets in a time-optimal manner. Techniques to dynamically allocate vehicles to targets and to find the time-optimal control actions of vehicles are proposed. The effectiveness of the time-optimal control technique is first demonstrated through numerical examples. The proposed strategy is then applied to a practical battlefield problem where ten vehicles are required to engage four targets, and numerical results show the efficiency of the proposed strategy.

This paper presents a minimum-time feedback controller for maneuvering a non-holonomic vehicle. A trajectory planning algorithm that generates minimum-time trajectories for moving a vehicle from an arbitrary starting location to the origin is presented. Trajectories generated are used to train a neural network that computes instantaneous velocity and steering commands as a function of the current vehicle state. The proposed strategy is illustrated by developing a neural network based controller for backing up a truck. Computer simulations are presented that demonstrates the effectiveness of the proposed technique in the presence of disturbances.

Three dimensional terrain maps are useful representations of environments for various robotic applications. Unfortunately, sensor data (from which such maps are built) is uncertain and contains errors which are usually not accounted for in existing terrain building algorithms. In real-time applications, it is necessary to quantify these uncertainties to allow map construction decisions to be made online. This paper addresses this issue by providing a representation that explicitly accounts for sensing uncertainty. This is achieved through the use of stochastic simulation techniques. The result is in an algorithm for online 3D multiresolution surface reconstruction of unknown, and unstructured environments. Results of the surface reconstruction algorithm in a real environment are presented.

The paper describes a terrain-aided navigation system that employs points of maximum curvature extracted from laser scan data as primary landmarks. A scale space method is used to extract points of maximum curvature from laser range scans of unmodified outdoor environments. This information is then fused with odometric information to provide localization information for an outdoor vehicle. The method described is invariant to the size and orientation of the range images under consideration (with respect to rotation and translation), is robust to noise, and can reliably detect and localize naturally occurring landmarks in the operating environment. The algorithm is demonstrated in the application of a road vehicle in an unmodified operating domain.

This paper presents a novel approach to the multi-vehicle simultaneous localisation and mapping (SLAM) problem that exploits the manner in which observations are fused into the global map of the environment to manage the computational complexity of the algorithm and improve the data association process. Rather than incorporating every observation directly into the global map of the environment, the constrained local submap filter (CLSF) relies on creating an independent, local submap of the features in the immediate vicinity of the vehicle. This local submap is then periodically fused into the global map of the environment. This representation is shown to reduce the computational complexity of maintaining the global map estimates as well as improving the data association process. This paper examines the prospect of applying the CLSF algorithm to the multi-vehicle SLAM problem.

This paper presents a novel approach to the simultaneous localisation and mapping algorithm that exploits the manner in which observations are fused into the global map of the environment to manage the computational complexity of the algorithm and improve the data association process. Rather than incorporating every observation directly into the global map of the environment, the constrained local submap filter relies on creating an independent, local submap of the features in the immediate vicinity of the vehicle. This local submap is then periodically fused into the global map of the environment using appropriately formulated constraints between the common feature estimates. This approach is shown to be effective in reducing the computational complexity of maintaining the global map estimates as well as improving the data association process.

The simultaneous localization and map building (SLAM) problem asks if it is possible for an autonomous vehicle to start in an unknown location in an unknown environment and then to incrementally build a map of this environment while simultaneously using this map to compute absolute vehicle location. Starting from estimation-theoretic foundations of this problem, the paper proves that a solution to the SLAM problem is indeed possible. The underlying structure of the SLAM problem is first elucidated. A proof that the estimated map converges monotonically to a relative map with zero uncertainty is then developed. It is then shown that the absolute accuracy of the map and the vehicle location reach a lower bound defined only by the initial vehicle uncertainty. Together, these results show that it is possible for an autonomous vehicle to start in an unknown location in an unknown environment and, using relative observations only, incrementally build a perfect map of the world and to compute simultaneously a bounded estimate of vehicle location. The paper also describes a substantial implementation of the SLAM algorithm on a vehicle operating in an outdoor environment using millimeter-wave radar to provide relative map observations. This implementation is used to demonstrate how some key issues such as map management and data association can be handled in a practical environment. The results obtained are cross-compared with absolute locations of the map landmarks obtained by surveying. In conclusion, the paper discusses a number of key issues raised by the solution to the SLAM problem including suboptimal map-building algorithms and map management.

This paper presents a new method for improving the accuracy of inertial measurement units (IMUs) mounted on land vehicles. The algorithm exploits nonholonomic constraints that govern the motion of a vehicle on a surface to obtain velocity observation measurements which aid in the estimation of the alignment of the IMU as well as the forward velocity of the vehicle. It is shown that this can be achieved without any external sensing provided that certain observability conditions are met. A theoretical analysis is provided together with a comparison of experimental results between a nonlinear implementation of the algorithm and an IMU/GPS navigation system. This comparison demonstrates the effectiveness of the algorithm. The real time implementation is also addressed through a multiple observation inertial aiding algorithm based on the information filter. The results show that the use of these constraints and vehicle speed guarantees the observability of the velocity and the attitude of the inertial unit, and hence bounds the errors associated with these states. The strategies proposed provides a tighter navigation loop which can sustain outages of GPS for a greater amount of time as compared to when the inertial unit is used with standard integration algorithms.

The theoretical basis of the solution to the simultaneous localisation and map building (SLAM) problem where an autonomous vehicle starts in an unknown location in an unknown environment and then incrementally build a map of landmarks present in this environment while simultaneously using this map to compute absolute vehicle location is well understood. Although a number of SLAM implementations have appeared in the literature, the need to maintain the knowledge of the relative relationships between all the landmark location estimates contained in the map makes SLAM computationally intractable in implementations containing more than few tens of landmarks. In this paper, the theoretical basis and a practical implementation of a computationally efficient solution to SLAM is presented. The paper shows that it is indeed possible to remove a large percentage of the landmarks from the map without making the map-building process statistically inconsistent. Furthermore, it is shown that the efficiency of the SLAM can be maintained by judicious selection of landmarks, to be preserved in the map, based on their information content.

We present results of the application of a simultaneous localisation and map building (SLAM) algorithm to estimate the motion of a submersible vehicle. Scans obtained from an on-board sonar are processed to extract stable point features in the environment. These point features are then used to build up a map of the environment while simultaneously providing estimates of the vehicle location. Results are shown from deployment in a swimming pool at the University of Sydney as well as from field trials in a natural environment along Sydney's coast. This work represents the first instance of a deployable underwater implementation of the SLAM algorithm.

Discusses the use of a 77 GHz millimetre wave radar as a guidance sensor for autonomous land vehicle navigation. An extended Kalman filter (EKF) maintains an estimate of map features in addition to vehicle state. This map augmented EKF is implemented with radar reflectors, designed for high visibility from all orientations. An improvement on this approach enables natural features to be identified, using the polarisation of returned radar signals. Those that are suitable as navigational markers are used to update estimates of vehicle and feature locations. This method requires no a priori knowledge of the environment and no target infrastructure.

Describes a real time, on-the-fly, roll and pitch alignment algorithm for inertial measurement units (IMUs) mounted on land vehicles. Unlike conventional strategies, the alignment is achieved without external observations. This is achieved by exploiting the nonholonomic constraints that govern the motion of a vehicle on a surface to obtain the roll and pitch of the IMU. The position of the vehicle along its path is still unobservable and the algorithm is effective only when there is sufficient excitation in all degrees of freedom. However, with the proposed alignment algorithm, the IMU is able to provide sufficiently accurate position information for substantially longer periods of time compared with conventional methods. Experimental results are provided along with a comparison of an IMU/GPS navigation loop showing the effectiveness of the algorithm.

This paper describes a series of trials that were done at an underground mine in New South Wales, Australia. Experimental results are presented from the data obtained during the field trials and suitable sensor suites for an autonomous mining vehicle navigation system are evaluated.

Describes the theoretical development and experimental evaluation of a navigation system for an autonomous load, haul, and dump truck based on the results obtained during extensive in-situ field trials. The particular contributions of the theoretical work are in designing the navigation system to be able to cope with vehicle slip in rough uneven terrain using information from inertial sensors, odometry, and a bearing only laser. Results are presented using data obtained during the field trials.

This paper reports on the map-building and map-based localization of a load-haul-dump (LHD) truck in an underground mine using statistical pattern-matching techniques utilizing range images obtained from a scanning laser range-finder The map-building approach is based on an extended Kalman filter (EKF) and the resulting map is composed of poly-lines. Three approaches are proposed for the localization of the vehicle, namely the iterative closest point (ICP) approach, a reflective beacon based approach and the combined ICP-EKF approach, wherein, the last two approaches explicitly take into account the uncertainty associated with the observation data. These approaches are then compared using data gathered from an underground mine in Queensland, Australia for their relative merits subject to various factors and the corresponding results are presented.

A crucial problem in crane control is to identify exactly the position and orientation of the load in space. This paper describes a new non-contact method for determining the crane load location by means of an inertial navigation system (INS) and a Kalman filter. The Kalman filter estimates the spreader position, which is not observed by the INS. Experiments were conducted on a 1/15th geometric scale model of a quay-crane. The work has potential application in the development of integrated estimation and control systems for full-scale quay-cranes.

A new approach for the autonomous navigation of a load-haul-dump (LHD) truck in an underground mine is presented. The development of a minimal-structure combined ICP-EKF algorithm utilizing a scanning-laser range-finder for the localization of the vehicle is described. The iterative closest point (ICP) algorithm is employed for matching the scanned data to an existing map in the form of a poly-line. This combined approach efficiently deals with the uncertainty present in the range data. An extended Kalman filter (EKF) algorithm is employed, that exploits a nonlinear kinematic model incorporating the vehicle-slip, a nonlinear observation model based on the vertices of the poly-line map, and the bearing of the laser-observations. This provides reliable vehicle estimates. Real data gathered during a trial run in the mine is employed in testing the efficiency of this approach which is found to be robust with respect to occlusions and outliers, demonstrating the successful navigation of the LHD.

This paper describes the design and implementation of a semi-autonomous, and ultimately fully autonomous, container quay-crane. The new crane is based on a novel reeving arrangement which allows both fast and accurate gross motion as well as fine micropositioning. The paper describes the essential theory behind this design and presents experimental results from a 1/15th scale model. The proposed instrumentation of this crane is also briefly described as are key elements of the operator interface.

This paper describes the theoretical development and experimental evaluation of a guidance system for an autonomous load, haul and dump truck (LHD) for use in underground mining. The particular contributions of this paper are in designing the navigation system to be able to cope with vehicle slip in rough uneven terrain using information from an inertial navigation system (INS) and a bearing only laser. Results are presented using data obtained during field trials.