This paper presents a novel Stochastic Optimal Control (SOC) method based on Model Predictive Path Integral control (MPPI), named Stein Variational Guided MPPI (SVG-MPPI), designed to handle rapidly shifting multimodal optimal action distributions. While MPPI can find a Gaussian-approximated optimal action distribution in closed form, i.e., without iterative solution updates, it struggles with the mul-timodality of the optimal distributions. This is due to the less representative nature of the Gaussian. To overcome this limitation, our method aims to identify a target mode of the optimal distribution and guide the solution to converge to fit it. In the proposed method, the target mode is roughly estimated using a modified Stein Variational Gradient Descent (SVGD) method and embedded into the MPPI algorithm to find a closed-form "mode-seeking" solution that covers only the target mode, thus preserving the fast convergence property of MPPI. Our simulation and real-world experimental results demonstrate that SVG-MPPI outperforms both the original MPPI and other state-of-the-art sampling-based SOC algorithms in terms of path-tracking and obstacle-avoidance capabilities. https://github.com/kohonda/proj-svg_mppi

This paper presents a reactive navigation method that leverages a Model Predictive Path Integral (MPPI) control enhanced with spline interpolation for the control input sequence and Stein Variational Gradient Descent (SVGD). The MPPI framework addresses a nonlinear optimization problem by determining an optimal sequence of control inputs through a sampling-based approach. The efficacy of MPPI is significantly influenced by the sampling noise. To rapidly identify routes that circumvent large and/or newly detected obstacles, it is essential to employ high levels of sampling noise. However, such high noise levels result in jerky control input sequences, leading to non-smooth trajectories. To mitigate this issue, we propose the integration of spline interpolation within the MPPI process, enabling the generation of smooth control input sequences despite the utilization of substantial sampling noises. Nonetheless, the standard MPPI algorithm struggles in scenarios featuring multiple optimal or near-optimal solutions, such as environments with several viable obstacle avoidance paths, due to its assumption that the distribution over an optimal control input sequence can be closely approximated by a Gaussian distribution. To address this limitation, we extend our method by incorporating SVGD into the MPPI framework with spline interpolation. SVGD, rooted in the optimal transportation algorithm, possesses the unique ability to cluster samples around an optimal solution. Consequently, our approach facilitates robust reactive navigation by swiftly identifying obstacle avoidance paths while maintaining the smoothness of the control input sequences. The efficacy of our proposed method is validated on simulations with a quadrotor, demonstrating superior performance over existing baseline techniques.

This paper presents a simultaneous localization and map-assisted environment recognition (SLAMER) method. Mobile robots usually have an environment map and environment information can be assigned to the map. Important information such as no entry zone can be predicted from the map if localization has succeeded. However, this prediction is failed when localization does not work. Uncertainty of pose estimate must be considered for robust-map-based environ-mental object prediction. Robots also have external sensors and can recognize environmental object; however, sensor-based recognition of course contain uncertainty. SLAMER fuses map-based prediction and sensor-based recognition while coping with these uncertainties and achieves accurate localization and environment recognition. In this paper, we demonstrate LiDAR-based implementation of SLAMER in two cases. In the first case, we use the SemanticKITTI dataset and show that SLAMER achieves accurate estimate more than traditional methods. In the second case, we use an indoor mobile robot and show that unmeasurable environmental objects such as open doors and no entry lines can be recognized.

Most of the recent automated driving systems assume the accurate functioning of localization. Unanticipated errors cause localization failures and result in failures in automated driving. An exact localization failure detection is necessary to ensure safety in automated driving; however, detection of the localization failures is challenging because sensor measurement is assumed to be independent of each other in the localization process. Owing to the assumption, the entire relation of the sensor measurement is ignored. Consequently, it is difficult to recognize the misalignment between the sensor measurement and the map when partial sensor measurement overlaps with the map. This paper proposes a method for the detection of localization failures using Markov random fields with fully connected latent variables. The full connection enables to take the entire relation into account and contributes to the exact misalignment recognition. Additionally, this paper presents localization failure probability calculation and efficient distance field representation methods. We evaluate the proposed method using two types of datasets. The first dataset is the SemanticKITTI dataset, whereby four methods are compared with the proposed method. The comparison results reveal that the proposed method achieves the most accurate failure detection. The second dataset is created based on log data acquired from the demonstrations that we conducted in Japanese public roads. The dataset includes several localization failure scenes. We apply the failure detection methods to the dataset and confirm that the proposed method achieves exact and immediate failure detection.

Recently, various applications leveraging topological data analysis, in particular, persistent homology (PH), have been presented in many fields since PH provides a novel point cloud analysis method. In this work, we apply PH to LiDAR-based ego-vehicle localization applications. PH can extract translation and rotation invariant features from a point cloud. These features do not maintain local information of the point cloud, such as the edges and lines; however, they can abstract the global structure of the point cloud. A persistence image (PI) vectorizes the features and allows us to obtain fixed-size vectors despite the sizes of the source point clouds being different. Additionally, the size of the PI is not large even though the source point cloud is extremely big. We consider that these advantages are effective to loop closure detection, place categorization, and end-to-end global localization applications. Results reveal that it is difficult to improve the localization accuracy by simply applying PH owing to the basic concept of the topology that does not focus on exact shapes of the geometry. Therefore, we discuss how the advantages of PH can be utilized for the localization.

This paper proposes a hybrid localization method that fuses Monte Carlo localization (MCL) and convolutional neural network (CNN)-based end-to-end (E2E) localization. MCL is based on particle filter and requires proposal distributions to sample the particles. The proposal distribution is generally predicted using a motion model. However, because the motion model cannot handle unanticipated errors, the predicted distribution is sometimes inaccurate. The use of other ideal proposal distributions, such as the measurement model, can improve robustness against such unanticipated errors. This technique is called importance sampling (IS). However, it is difficult to sample the particles from such ideal distributions because they are not represented in the closed form. Recent works have proved that CNNs with dropout layers represent the posterior distributions over their outputs conditioned on the inputs and the CNN predictions are equivalent to sampling the outputs from the posterior. Therefore, the proposed method utilizes a CNN to sample the particles and fuses them with MCL via IS. Consequently, the advantages of both MCL and E2E localization can be simultaneously leveraged while preventing their disadvantages. Experiments demonstrate that the proposed method can smoothly estimate the robot pose, similar to the model-based method, and quickly re-localize it from the failures, similar to the learning-based method.

This paper presents a LiDAR-based 3D Monte Carlo localization (MCL) with an efficient distance field (DF) representation method. To implement 3D MCL, high computing capacity is required because the likelihood of many pose candidates, i.e., particles, must be calculated in real time by comparing sensor measurements and a map. Additionally, a large-scale map is needed for allocation to embedded computers since autonomous vehicles are required to navigate wide areas. These make it difficult for 3D MCL implementation. This paper first presents an efficient DF representation method while considering the 3D LiDAR-based localization characteristics. Because each DF voxel has the closest distance from occupied voxels, swift comparison of the sensor measurements and map can be achieved. Consequently, 3D MCL using the likelihood field model (LFM) can be executed in real time. Furthermore, this paper presents a method for improving the localization robustness to environmental changes without increasing memory and computational cost from that of the LFM-based MCL. Through experiments using the SemanticKITTI dataset, we show that the presented method can efficiently and robustly work in dynamic environments.

Interaction detection between vehicles and vulnerable road users (e.g. pedestrians and cyclists) is important for e.g. safety control and autonomous driving. However, there are many challenges for automatically detecting interactions, such as the ambiguity of defining when interaction is required in dynamic traffic activities among different road users and the lack of labeled data for training a machine learning detector. To overcome the challenges, we introduce a way to define whether or not interaction is required in various traffic scenes and create a large real-world dataset from a very challenging intersection. A sequence-to-sequence method that uses the object information and motion information of the traffic scenes extracted by a state-of-the-art object detector and from optical flow, respectively, is proposed for automatic interaction detection. The proposed method generates a probability of interaction at each short interval (<; 0.1 s) that represents the changing of interaction along a sequence. We obtain a baseline model that differentiates no interaction from interaction on the basis of the location and road user type from the detected object information. Compared with the baseline model, the empirical results of the proposed method demonstrate very accurate predictions for vehicle turning sequences with varying length.

This paper presents a comparison of driving behavior modeling methods based on hidden Markov models (HMMs) with driver's eye-gaze measurement and ego-vehicle localization. Original HMMs are sometimes insufficient to model real-world scenarios. To overcome these limitations, extended HMMs have been proposed, e.g., autoregressive input-output HMMs (AIOHMMs). This paper first details AIOHMMs and presents ways to use them for driving behavior modeling. We compare the performance for behavior modeling and maneuver discrimination for six types of HMMs. The driving data for this work was gathered in our university campus with a car-like vehicle. Experimental results suggest that the hidden states can properly represent the average of the driving actions when the driving behaviors are accurately modeled by the HMMs. It is also suggested that surrounding and past information can be used to flexibly model the relationship between driving actions and related information.

Localization robustness against environment dynamics is significant for robots to achieve autonomous navigation in unmodified environments. A basic method of improving the robustness of a robot is considering the sensor observations obtained from mapped obstacles and using them for localizing the robot's pose. This study proposes an observation model that considers the class of sensor observations, where “class” categorizes the sensor observations as those obtained from mapped and unmapped obstacles. In the proposed approach, the robot's pose and the class are estimated simultaneously. As a result, the robot's pose can be localized using the sensor observations obtained only from mapped obstacles. First, we evaluated the performance of the proposed approach using simulations. Further, we tested the proposed approach in a real-world mobile robot navigation competition, called “Tsukuba Challenge,” held in Japan. The robustness and effectiveness of the proposed approach against environment dynamics were verified from the experimental results.

In this paper we present a data driven approach for safe and smooth autonomous navigation of a personal mobility vehicle (PMV) when facing moving obstacles such as people and bicycles in public pedestrian paths. In a period of three months, data from five different persons driving the robotic PMV in an outdoor environment while facing pedestrians were collected. 2465 clean tracks around the vehicle together with PMVs trajectories were collected. We performed an analysis of the parameters involved for human-driven smooth navigation. Relevant parameters regarding PMV-Human interaction included distance to moving objects, passing side and velocities. Moreover, data suggests the existence of a social navigational distance for the PWv. For autonomous navigation we implemented a Frenet planner to achieve safe and smooth navigation for the passenger and pedestrians around. Experimental results in real pedestrian paths show that the PMV is capable of smoothly following its path while facing pedestrians and bicycles.

This paper proposes a method for estimation of the reliability of vehicle localization results. We previously proposed a fault detection method for indoor mobile robots using a convolutional neural network (CNN). Because image data is generally fed to a CNN, we feed image data obtained from the robot pose, occupancy grid map, and laser scan data to the CNN, which decides of whether localization has failed. The previous method also employed a Rao-Blackwellized particle filter to estimate the robot pose and reliability of this estimation simultaneously. However, it was difficult for vehicle robots to use the previous method as creating and processing image data is not a light computation process. In this study, we extend the previous method by improving the data fed to the CNN, thus making it possible for vehicle robots to perform simultaneous localization and estimation. This paper describes in detail the simultaneous estimation and shows that the reliability can be used as an exact criterion for detecting localization failures. Keywords-Vehicle Localization, Reliability

In this paper we present a method to learn how to drive in different types of blind intersections using expert driving data. We cluster different intersections based on the velocity of how drivers approach them, and train a linear SVM classifier for each class of intersection. Through clustering we found that there were three different classes of intersections in typical residential areas in Japan. We used inverse reinforcement learning (IRL) to build a driving model for each type of intersection. The models were trained from 308 trajectories traversed by 5 different drivers. The models and policies were implemented and evaluated in a ROS simulator where the agent is provided a global path, and upon it reaching an intersection, it selects the appropriate trained policy. By doing this, the simulated autonomous vehicle can perform proactive safe driving behaviors when approaching blind intersections.

Magnetic fields are used for localization and navigation in the field of robotics. In recent years, because of the spread of mobile devices equipped with magnetic sensors (e.g., smart phones), the use of magnetic fields has been extensive, especially for position tracking of mobile devices. One example application of such tracking is in identifying the position of a person with a mobile device. Development of this application requires a three-dimensional (3D) magnetic map that represents the magnetic distribution of a 3D environment since the device moves around in 3D space. It is, however, difficult to construct a 3D magnetic map of a large-scale environment because measuring the magnetic field is time consuming and expensive. In this paper we propose an efficient method for mapping the 3D magnetic field of a large-scale environment. The method uses a mobile manipulator to measure the 3D magnetic field, enabling 3D magnetic data to be automatically collected. Moreover, the method uses Gaussian processes (GPs) for regression of the magnetic field. In this study, we first evaluate the performance of the GPs and then describe the measurement robot. In an experiment, a 3D magnetic field of an indoor environment is visualized by using this method and the performance of the presented method is demonstrated.

This paper presents a model for safe driving at blind intersections and its integration to a local planner based on a Frenet frame. The model predicts potential moving obstacles from blind intersections to proactively slow down to avoid potential collisions. The derivation of the model is described and its parameters are detailed. The local planner computes smooth trajectories with smooth velocity profiles so that the vehicle can follow the paths without jerk and sudden accelerations resulting in safe and comfortable navigation. Experimental results in simulation and in the real field with an autonomous car, show that the proposed predictive driving framework can reproduce human expert driver's trajectories and velocities when facing blind intersections.

In this paper, we present a localization approach that is based on a point-cloud matching method (normal distribution transform “NDT”) and road-marker matching based on the light detection and ranging intensity. Point-cloud map-based localization methods enable autonomous vehicles to accurately estimate their own positions. However, accurate localization and “matching error” estimations cannot be performed when the appearance of the environment changes, and this is common in rural environments. To cope with these inaccuracies, in this work, we propose to estimate the error of NDT scan matching beforehand (off-line). Then, as the vehicle navigates in the environment, the appropriate uncertainty is assigned to the scan matching. 3D NDT scan matching utilizes the uncertainty information that is estimated off-line, and is combined with a road-marker matching approach using a particle-filtering algorithm. As a result, accurate localization can be performed in areas in which 3D NDT failed. In addition, the uncertainty of the localization is reduced. Experimental results show the performance of the proposed method.

This paper presents a model for velocity control in blind corners and intersections based on expert driver data. Accurate expert driver data was collected with a car equipped with a 3D LiDAR and high definition maps. A model based on human expert driver data is used to control the velocity of the ego-vehicle when facing blind intersections. The model regulates ego-vehicle velocity based on the visibility of the road at the blind intersection. As the vehicle approximates the intersection and crossing roads are not visible, the vehicle slows down, then as the roads become visible the vehicle accelerates. Experimental results show the performance of the velocity model compared towards 270 trajectories taken from 7 expert drivers towards 6 different intersections without mandatory stops.

The magnetic field that exists in an indoor environment includes rich magnetic fluctuations because buildings contain many magnetized materials (e.g., steel frames). These fluctuations can be used as landmarks, the use of which requires the creation of a magnetic map representing the distribution of the magnetic field. It is, however, difficult to build a large-scale magnetic map because of the narrow measurement range of a magnetic sensor. This paper proposes an efficient method for collecting magnetic data using a mobile robot and a method for building a magnetic map using Gaussian processes. The use of these methods make it possible to build a large-scale magnetic map efficiently. Moreover, this paper presents a particle filter-based localization method based on the magnetic map. The presented system enables a robot to identify its own position in large-scale buildings. Experiments are used to demonstrate the performance and usefulness of the presented system.