Probabilistic Self-Localization and Mapping: An Asynchronous Multirate Approach

"© 2008 IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works."[EN] In this paper, we present a set of robust and efficient algorithms with O(N) cost for the solution of the Simultaneous Localization And Mapping (SLAM) problem of a mobile robot. First, we introduce a novel object detection method, which is mainly based on multiple line fitting method for landmark detection with regular constrained angles. Second, a line-based pose estimation method is proposed, based on LeastSquares (LS). This method performs the matching of lines, providing the global pose estimation under assumption of known Data-Association. Finally, we extend the FastSLAM (FActored Solution To SLAM) algorithm for mobile robot self-localisation and mapping by considering the asynchronous sampling of sensors and actuators. In this sense, multi-rate asynchronous holds are used to interface signals with different sampling rates. Moreover, an asynchronous fusion method to predict and update mobile robot pose and map is also presented. In addition to this, FastSLAM 1.0 has been also improved by considering the estimated pose with the LS-approach to re-allocate each particle of the posterior distribution of the robot pose. This approach has a lower computational cost than the original Extended Kalman Filtering (EKF) approach in FastSLAM 2.0. All these methods have been combined in order to perform an efficient and robust self-localization and map building process. Additionally, these methods have been validated with experimental real data, in mobile robot moving on an unknown environment for solving the SLAM problem.This work has been supported by the Spanish Government (MCyT) research project BIA2005-09377-C03-02 and by the Italian Government (MIUR) research project PRIN2005097207.Armesto, L.; Ippoliti, G.; Longhi, S.; Tornero Montserrat, J. (2008). Probabilistic Self-Localization and Mapping: An Asynchronous Multirate Approach. IEEE Robotics & Automation Magazine. 15(2):77-88. https://doi.org/10.1109/M-RA.2007.907355S778815

Grid-based Hybrid 3DMA GNSS and Terrestrial Positioning

The paper discusses the increasing use of hybridized sensor information for GNSS-based localization and navigation, including the use of 3D map-aided GNSS positioning and terrestrial systems based on different geometric measurement principles. However, both GNSS and terrestrial systems are subject to negative impacts from the propagation environment, which can violate the assumptions of conventionally applied parametric state estimators. Furthermore, dynamic parametric state estimation does not account for multi-modalities within the state space leading to an information loss within the prediction step. In addition, the synchronization of non-deterministic multi-rate measurement systems needs to be accounted. In order to address these challenges, the paper proposes the use of a non-parametric filtering method, specifically a 3DMA multi-epoch Grid Filter, for the tight integration of GNSS and terrestrial signals. Specifically, the fusion of GNSS, Ultra-wide Band (UWB) and vehicle motion data is introduced based on a discrete state representation. Algorithmic challenges, including the use of different measurement models and time synchronization, are addressed. In order to evaluate the proposed method, real-world tests were conducted on an urban automotive testbed in both static and dynamic scenarios. We empirically show that we achieve sub-meter accuracy in the static scenario by averaging a positioning error of $0.64$ m, whereas in the dynamic scenario the average positioning error amounts to $1.62$ m. The paper provides a proof-of-concept of the introduced method and shows the feasibility of the inclusion of terrestrial signals in a 3DMA positioning framework in order to further enhance localization in GNSS-degraded environments

Asynchronous sensor fusion of GPS, IMU and CAN-based odometry for heavy-duty vehicles

In heavy-duty vehicles, multiple signals are available to estimate the vehicle's kinematics, such as Inertial Measurement Unit (IMU), Global Positioning System (GPS) and linear and angular speed readings from wheel tachometers on the internal Controller Area Network (CAN). These signals have different noise variance, bandwidth and sampling rate (being the latter, possibly, irregular). In this paper we present a non-linear sensor fusion algorithm allowing asynchronous sampling and non-causal smoothing. It is applied to achieve accuracy improvements when incorporating odometry measurements from CAN bus to standard GPS+IMU kinematic estimation, as well as the robustness against missing data. Our results show that this asynchronous multi-sensor (GPS+IMU+CAN-based odometry) fusion is advantageous in low-speed manoeuvres, improving accuracy and robustness to missing data, thanks to non-causal filtering. The proposed algorithm is based on Extended Kalman Filter and Smoother, with exponential discretization of continuous-time stochastic differential equations, in order to process measurements at arbitrary time instants; it can provide data to subsequent processing steps at arbitrary time instants, not necessarily coincident with the original measurement ones. Given the extra information available in the smoothing case, its estimation performance is less sensitive to the noise-variance parameter setting, compared to causal filtering. Working Matlab code is provided at the end of this work

PWM and PFM for visual servoing in fully decoupled approaches

In this paper, novel visual servoing techniques based on Pulse Width Modulation (PWM) and Pulse Frequency Modulation (PFM) are presented. In order to apply previous pulse modulations, a fully decoupled position based visual servoing approach (i.e. with block-diagonal interaction matrix) is considered, controlling independently translational and rotational camera motions. These techniques, working at high frequency, could be considered to address the sensor latency problem inherent in visual servoing systems. The expected appearance of ripple due to the concentration of the control action in pulses is quantified and analyzed under simulated scenario. This high frequency ripple does not affect the system performance since it is filtered by the manipulator dynamics. On the contrary it can be seen as a dither signal to minimize the impact of friction and overcome back-lashing.This work was supported in part by the Spanish Government under Grant BES-2010-038486 and Project DPI2013-42302-R.Muñoz Benavent, P.; Solanes Galbis, JE.; Gracia Calandin, LI.; Tornero Montserrat, J. (2015). PWM and PFM for visual servoing in fully decoupled approaches. Robotics and Autonomous Systems. 65(1):57-64. doi:10.1016/j.robot.2014.11.011S576465

Predicting Trajectory Paths For Collision Avoidance Systems

This work was motivated by the idea of developing a more encompassing collision avoidance system that supported vehicle to vehicle (V2V) and vehicle to infrastructure (V2I) communications. Current systems are mostly based on line of sight sensors that are used to prevent a collision, but these systems would prevent even more accidents if they could detect possible collisions before both vehicles were in line of sight. For this research we concentrated mostly on the aspect of improving the prediction of a vehicle\u27s future trajectory, particularly on non-straight paths. Having an accurate prediction of where the vehicle is heading is crucial for the system to reliably determine possible path intersections of more than one vehicle at the same time. We first evaluated the benefits of merging Global Positioning System (GPS) data with the Geographical Information System (GIS) data to correct improbable predicted positions. We then created a new algorithm called the Dead Reckoning with Dynamic Errors (DRWDE) sensor fusion, which can predict future positions at the rate of its fastest sensor, while improving the handling of accumulated error while some of the sensors are offline for a given period of time. The last part of out research consisted in the evaluation of the use of smartphones\u27 built-in sensors to predict a vehicle\u27s trajectory, as a possible intermediate solution for a vehicle to vehicle (V2V) and vehicle to infrastructure (V2I) communications, until all vehicles have all the necessary sensors and communication infrastructure to fully populate this new system. For the first part of our research, the actual experimental results validated our proposed system, which reduced the position prediction errors during curves to around half of what it would be without the use of GIS data for prediction corrections. The next improvement we worked on was the ability to handle change in noise, depending on unavailable sensor measurements, permitting a flexibility to use any type of sensor and still have the system run at the fastest frequency available. Compared to a more common KF implementation that run at the rate of its slowest sensor (1Hz in our setup), our experimental results showed that our DRWDE (running at 10Hz) yielded more accurate predictions (25-50% improvement) during abrupt changes in the heading of the vehicle. The last part of our research showed that, comparing to results obtained with the vehicle-mounted sensors, some smartphones yield similar prediction errors and can be used to predict a future position

MULTI-RATE VISUAL FEEDBACK ROBOT CONTROL

[EN] This thesis deals with two characteristic problems in visual feedback robot control: 1) sensor latency; 2) providing suitable trajectories for the robot and for the measurement in the image. All the approaches presented in this work are analyzed and implemented on a 6 DOF industrial robot manipulator or/and a wheeled robot. Focusing on the sensor latency problem, this thesis proposes the use of dual-rate high order holds within the control loop of robots. In this sense, the main contributions are: - Dual-rate high order holds based on primitive functions for robot control (Chapter 3): analysis of the system performance with and without the use of this multi-rate technique from non-conventional control. In addition, as consequence of the use of dual-rate holds, this work obtains and validates multi-rate controllers, especially dual-rate PIDs. - Asynchronous dual-rate high order holds based on primitive functions with time delay compensation (Chapter 3): generalization of asynchronous dual-rate high order holds incorporating an input signal time delay compensation component, improving thus the inter-sampling estimations computed by the hold. It is provided an analysis of the properties of such dual-rate holds with time delay compensation, comparing them with estimations obtained by the equivalent dual-rate holds without this compensation, as well as their implementation and validation within the control loop of a 6 DOF industrial robot manipulator. - Multi-rate nonlinear high order holds (Chapter 4): generalization of the concept of dual-rate high order holds with nonlinear estimation models, which include information about the plant to be controlled, the controller(s) and sensor(s) used, obtained from machine learning techniques. Thus, in order to obtain such a nonlinear hold, it is described a methodology non dependent of the machine technique used, although validated using artificial neural networks. Finally, an analysis of the properties of these new holds is carried out, comparing them with their equivalents based on primitive functions, as well as their implementation and validation within the control loop of an industrial robot manipulator and a wheeled robot. With respect to the problem of providing suitable trajectories for the robot and for the measurement in the image, this thesis presents the novel reference features filtering control strategy and its generalization from a multi-rate point of view. The main contributions in this regard are: - Reference features filtering control strategy (Chapter 5): a new control strategy is proposed to enlarge significantly the solution task reachability of robot visual feedback control. The main idea is to use optimal trajectories proposed by a non-linear EKF predictor-smoother (ERTS), based on Rauch-Tung-Striebel (RTS) algorithm, as new feature references for an underlying visual feedback controller. In this work it is provided both the description of the implementation algorithm and its implementation and validation utilizing an industrial robot manipulator. - Dual-rate Reference features filtering control strategy (Chapter 5): a generalization of the reference features filtering approach from a multi-rate point of view, and a dual Kalman-smoother step based on the relation of the sensor and controller frequencies of the reference filtering control strategy is provided, reducing the computational cost of the former algorithm, as well as addressing the problem of the sensor latency. The implementation algorithms, as well as its analysis, are described.[ES] La presente tesis propone soluciones para dos problemas característicos de los sistemas robóticos cuyo bucle de control se cierra únicamente empleando sensores de visión artificial: 1) la latencia del sensor; 2) la obtención de trayectorias factibles tanto para el robot así como para las medidas obtenidas en la imagen. Todos los métodos propuestos en este trabajo son analizados, validados e implementados utilizando brazo robot industrial de 6 grados de libertad y/o en un robot con ruedas. Atendiendo al problema de la latencia del sensor, esta tesis propone el uso de retenedores bi-frequencia de orden alto dentro de los lazos de control de robots. En este aspecto las principales contribuciones son: -Retenedores bi-frecuencia de orden alto basados en funciones primitivas dentro de lazos de control de robots (Capítulo 3): análisis del comportamiento del sistema con y sin el uso de esta técnica de control no convencional. Además, como consecuencia del empleo de los retenedores, obtención y validación de controladores multi-frequencia, concretamente de PIDs bi-frecuencia. -Retenedores bi-frecuencia asíncronos de orden alto basados en funciones primitivas con compensación de retardos (Capítulo 3): generalización de los retenedores bi-frecuencia asíncronos de orden alto incluyendo una componente de compensación del retardo en la señal de entrada, mejorando así las estimaciones inter-muestreo calculadas por el retenedor. Se proporciona un análisis de las propiedades de los retenedores con compensación del retardo, comparándolas con las obtenidas por sus predecesores sin compensación, así como su implementación y validación en un brazo robot de 6 grados de libertad. -Retenedores multi-frecuencia no lineales de orden alto (Capítulo 4): generalización del concepto de retenedor bi-frecuencia de orden alto con modelos de estimación no lineales, los cuales incluyen información tanto de la planta a controlar, como del controlador(es) y sensor(es) empleado(s), obtenida a partir de técnicas de aprendizaje. Así pues, para obtener dicho retenedor no lineal, se describe una metodología independiente de la herramienta de aprendizaje utilizada, aunque validada con el uso de redes neuronales artificiales. Finalmente se realiza un análisis de las propiedades de estos nuevos retenedores, comparándolos con sus predecesores basados en funciones primitivas, así como su implementación y validación en un brazo robot de 6 grados de libertad y en un robot móvil con ruedas. Por lo que respecta al problema de generación de trayectorias factibles para el robot y para la medida en la imagen, esta tesis propone la nueva estrategia de control basada en el filtrado de la referencia y su generalización desde el punto de vista multi-frecuencial. -Estrategia de control basada en el filtrado de la referencia (Capítulo 5): una nueva estrategia de control se propone para ampliar significativamente el espacio de soluciones de los sistemas robóticos realimentados con sensores de visión artificial. La principal idea es utilizar las trayectorias óptimas obtenidas por una trayectoria predicha por un filtro de Kalman seguido de un suavizado basado en el algoritmo Rauch-Tung-Striebel (RTS) como nuevas referencias para un controlador dado. En este trabajo se proporciona tanto la descripción del algoritmo como su implementación y validación empleando un brazo robótico industrial. -Estrategia de control bi-frecuencia basada en el filtrado de la referencia (Capítulo 5): generalización de la estrategia de control basada en filtrado de la referencia desde un punto de vista multi-frecuencial, con un filtro de Kalman multi-frecuencia y un Kalman-smoother dual basado en la relación existente entre las frecuencias del sensor y del controlador, reduciendo así el coste computacional del algoritmo y, al mismo tiempo, dando solución al problema de la latencia del sensor. La validación se realiza utilizando un barzo robot industria asi[CA] La present tesis proposa solucions per a dos problemes característics dels sistemes robòtics el els que el bucle de control es tanca únicament utilitzant sensors de visió artificial: 1) la latència del sensor; 2) l'obtenció de trajectòries factibles tant per al robot com per les mesures en la imatge. Tots els mètodes proposats en aquest treball son analitzats, validats e implementats utilitzant un braç robot industrial de 6 graus de llibertat i/o un robot amb rodes. Atenent al problema de la latència del sensor, esta tesis proposa l'ús de retenidors bi-freqüència d'ordre alt a dins del llaços de control de robots. Al respecte, les principals contribucions son: - Retenidors bi-freqüència d'ordre alt basats en funcions primitives a dintre dels llaços de control de robots (Capítol 3): anàlisis del comportament del sistema amb i sense l'ús d'aquesta tècnica de control no convencional. A més a més, com a conseqüència de l'ús dels retenidors, obtenció i validació de controladors multi-freqüència, concretament de PIDs bi-freqüència. - Retenidors bi-freqüència asíncrons d'ordre alt basats en funcions primitives amb compensació de retards (Capítol 3): generalització dels retenidors bi-freqüència asíncrons d'ordre alt inclouen una component de compensació del retràs en la senyal d'entrada al retenidor, millorant així les estimacions inter-mostreig calculades per el retenidor. Es proporciona un anàlisis de les propietats dels retenidors amb compensació del retràs, comparant-les amb les obtingudes per el seus predecessors sense la compensació, així com la seua implementació i validació en un braç robot industrial de 6 graus de llibertat. - Retenidors multi-freqüència no-lineals d'ordre alt (Capítol 4): generalització del concepte de retenidor bi-freqüència d'ordre alt amb models d'estimació no lineals, incloent informació tant de la planta a controlar, com del controlador(s) i sensor(s) utilitzat(s), obtenint-la a partir de tècniques d'aprenentatge. Així doncs, per obtindre el retenidor no lineal, es descriu una metodologia independent de la ferramenta d'aprenentatge utilitzada, però validada amb l'ús de rets neuronals artificials. Finalment es realitza un anàlisis de les propietats d'aquestos nous retenidors, comparant-los amb els seus predecessors basats amb funcions primitives, així com la seua implementació i validació amb un braç robot de 6 graus de llibertat i amb un robot mòbil de rodes. Per el que respecta al problema de generació de trajectòries factibles per al robot i per la mesura en la imatge, aquesta tesis proposa la nova estratègia de control basada amb el filtrat de la referència i la seua generalització des de el punt de vista multi-freqüència. - Estratègia de control basada amb el filtrat de la referència (Capítol 5): una nova estratègia de control es proposada per ampliar significativament l'espai de solucions dels sistemes robòtics realimentats amb sensors de visió artificial. La principal idea es la d'utilitzar les trajectòries optimes obtingudes per una trajectòria predita per un filtre de Kalman seguit d'un suavitzat basat en l'algoritme Rauch-Tung-Striebel (RTS) com noves referències per a un control donat. En aquest treball es proporciona tant la descripció del algoritme així com la seua implementació i validació utilitzant un braç robòtic industrial de 6 graus de llibertat. - Estratègia de control bi-freqüència basada en el filtrat (Capítol 5): generalització de l'estratègia de control basada am filtrat de la referència des de un punt de vista multi freqüència, amb un filtre de Kalman multi freqüència i un Kalman-Smoother dual basat amb la relació existent entre les freqüències del sensor i del controlador, reduint així el cost computacional de l'algoritme i, al mateix temps, donant solució al problema de la latència del sensor. L'algoritme d'implementació d'aquesta tècnica, així com la seua validaciSolanes Galbis, JE. (2015). MULTI-RATE VISUAL FEEDBACK ROBOT CONTROL [Tesis doctoral no publicada]. Universitat Politècnica de València. https://doi.org/10.4995/Thesis/10251/57951TESI

From SLAM to Situational Awareness: Challenges and Survey

The knowledge that an intelligent and autonomous mobile robot has and is able to acquire of itself and the environment, namely the situation, limits its reasoning, decision-making, and execution skills to efficiently and safely perform complex missions. Situational awareness is a basic capability of humans that has been deeply studied in fields like Psychology, Military, Aerospace, Education, etc., but it has barely been considered in robotics, which has focused on ideas such as sensing, perception, sensor fusion, state estimation, localization and mapping, spatial AI, etc. In our research, we connected the broad multidisciplinary existing knowledge on situational awareness with its counterpart in mobile robotics. In this paper, we survey the state-of-the-art robotics algorithms, we analyze the situational awareness aspects that have been covered by them, and we discuss their missing points. We found out that the existing robotics algorithms are still missing manifold important aspects of situational awareness. As a consequence, we conclude that these missing features are limiting the performance of robotic situational awareness, and further research is needed to overcome this challenge. We see this as an opportunity, and provide our vision for future research on robotic situational awareness.Comment: 15 pages, 8 figure

Control Theory: A Mathematical Perspective on Cyber-Physical Systems

Control theory is an interdisciplinary field that is located at the crossroads of pure and applied mathematics with systems engineering and the sciences. Recently the control field is facing new challenges motivated by application domains that involve networks of systems. Examples are interacting robots, networks of autonomous cars or the smart grid. In order to address the new challenges posed by these application disciplines, the special focus of this workshop has been on the currently very active field of Cyber-Physical Systems, which forms the underlying basis for many network control applications. A series of lectures in this workshop was devoted to give an overview on current theoretical developments in Cyber-Physical Systems, emphasizing in particular the mathematical aspects of the field. Special focus was on the dynamics and control of networks of systems, distributed optimization and formation control, fundamentals of nonlinear interconnected systems, as well as open problems in control

Signals on Networks: Random Asynchronous and Multirate Processing, and Uncertainty Principles

The processing of signals defined on graphs has been of interest for many years, and finds applications in a diverse set of fields such as sensor networks, social and economic networks, and biological networks. In graph signal processing applications, signals are not defined as functions on a uniform time-domain grid but they are defined as vectors indexed by the vertices of a graph, where the underlying graph is assumed to model the irregular signal domain. Although analysis of such networked models is not new (it can be traced back to the consensus problem studied more than four decades ago), such models are studied recently from the view-point of signal processing, in which the analysis is based on the "graph operator" whose eigenvectors serve as a Fourier basis for the graph of interest. With the help of graph Fourier basis, a number of topics from classical signal processing (such as sampling, reconstruction, filtering, etc.) are extended to the case of graphs. The main contribution of this thesis is to provide new directions in the field of graph signal processing and provide further extensions of topics in classical signal processing. The first part of this thesis focuses on a random and asynchronous variant of "graph shift," i.e., localized communication between neighboring nodes. Since the dynamical behavior of randomized asynchronous updates is very different from standard graph shift (i.e., state-space models), this part of the thesis focuses on the convergence and stability behavior of such random asynchronous recursions. Although non-random variants of asynchronous state recursions (possibly with non-linear updates) are well-studied problems with early results dating back to the late 60's, this thesis considers the convergence (and stability) in the statistical mean-squared sense and presents the precise conditions for the stability by drawing parallels with switching systems. It is also shown that systems exhibit unexpected behavior under randomized asynchronicity: an unstable system (in the synchronous world) may be stabilized simply by the use of randomized asynchronicity. Moreover, randomized asynchronicity may result in a lower total computational complexity in certain parameter settings. The thesis presents applications of the random asynchronous model in the context of graph signal processing including an autonomous clustering of network of agents, and a node-asynchronous communication protocol that implements a given rational filter on the graph. The second part of the thesis focuses on extensions of the following topics in classical signal processing to the case of graph: multirate processing and filter banks, discrete uncertainty principles, and energy compaction filters for optimal filter design. The thesis also considers an application to the heat diffusion over networks. Multirate systems and filter banks find many applications in signal processing theory and implementations. Despite the possibility of extending 2-channel filter banks to bipartite graphs, this thesis shows that this relation cannot be generalized to M-channel systems on M-partite graphs. As a result, the extension of classical multirate theory to graphs is nontrivial, and such extensions cannot be obtained without certain mathematical restrictions on the graph. The thesis provides the necessary conditions on the graph such that fundamental building blocks of multirate processing remain valid in the graph domain. In particular, it is shown that when the underlying graph satisfies a condition called M-block cyclic property, classical multirate theory can be extended to the graphs. The uncertainty principle is an essential mathematical concept in science and engineering, and uncertainty principles generally state that a signal cannot have an arbitrarily "short" description in the original basis and in the Fourier basis simultaneously. Based on the fact that graph signal processing proposes two different bases (i.e., vertex and the graph Fourier domains) to represent graph signals, this thesis shows that the total number of nonzero elements of a graph signal and its representation in the graph Fourier domain is lower bounded by a quantity depending on the underlying graph. The thesis also presents the necessary and sufficient condition for the existence of 2-sparse and 3-sparse eigenvectors of a connected graph. When such eigenvectors exist, the uncertainty bound is very low, tight, and independent of the global structure of the graph. The thesis also considers the classical spectral concentration problem. In the context of polynomial graph filters, the problem reduces to the polynomial concentration problem studied more generally by Slepian in the 70's. The thesis studies the asymptotic behavior of the optimal solution in the case of narrow bandwidth. Different examples of graphs are also compared in order to show that the maximum energy compaction and the optimal filter depends heavily on the graph spectrum. In the last part, the thesis considers the estimation of the starting time of a heat diffusion process from its noisy measurements when there is a single point source located on a known vertex of a graph with unknown starting time. In particular, the Cramér-Rao lower bound for the estimation problem is derived, and it is shown that for graphs with higher connectivity the problem has a larger lower bound making the estimation problem more difficult.</p