Practical Bayes filters often assume the state distribution of each time step to be Gaussian for computational tractability, resulting in the so-called Gaussian filters. When facing nonlinear systems, Gaussian filters such as extended Kalman filter (EKF) or unscented Kalman filter (UKF) typically rely on certain linearization techniques, which can introduce large estimation errors. To address this issue, this paper reconstructs the prediction and update steps of Gaussian filtering as solutions to two distinct optimization problems, whose optimal conditions are found to have analytical forms from Stein's lemma. It is observed that the stationary point for the prediction step requires calculating the first two moments of the prior distribution, which is equivalent to that step in existing moment-matching filters. In the update step, instead of linearizing the model to approximate the stationary points, we propose an iterative approach to directly minimize the update step's objective to avoid linearization errors. For the purpose of performing the steepest descent on the Gaussian manifold, we derive its natural gradient that leverages Fisher information matrix to adjust the gradient direction, accounting for the curvature of the parameter space. Combining this update step with moment matching in the prediction step, we introduce a new iterative filter for nonlinear systems called Natural Gradient Gaussian Approximation filter, or NANO filter for short. We prove that NANO filter locally converges to the optimal Gaussian approximation at each time step. Furthermore, the estimation error is proven exponentially bounded for nearly linear measurement equation and low noise levels through constructing a supermartingale-like property across consecutive time steps. Real-world experiments demonstrate that, compared to popular Gaussian filters such as EKF, UKF, iterated EKF, and posterior linearization filter, NANO filter reduces the average root mean square error by approximately 45% while maintaining a comparable computational burden.

Motion planning for large-scale robotic swarms presents significant challenges in terms of scalability and safety assurance in cluttered environments. To address these issues, this manuscript proposes a Closed-loop hierarchical Risk-aware swarm mOtion planner using Conditional ValuE at Risk (C-ROVER) that enables safe and efficient navigation for swarm robotic systems. The hierarchical structure of C-ROVER comprises a macroscopic planning stage that models the swarm state with Gaussian Mixture Models (GMMs) and generates trajectories for the swarm GMM, followed by a microscopic control stage that computes individual robot control using distributed model predictive control to track the GMM trajectories while achieving robot-level collision avoidance. Robot positions are periodically used to update the swarm GMM, closing the hierarchical planning and control loop. To achieve collision risk-awareness between the swarm and environmental obstacles at the macroscopic stage, C-ROVER leverages the stochastic Signed Distance Function to characterize the distance between the swarm GMM and obstacles, which is proven to follow a GMM. Then C-ROVER proposes an analytical expression of Conditional Value-at-Risk (CVaR) of a GMM to enable the swarm collision risk mitigation. Furthermore, C-ROVER designs a novel risk-aware space discretization approach to enhance the ability to navigate constrained spaces. To achieve efficient online motion planning, C-ROVER develops a convergent sequential convex programming approach for macroscopic planning, leveraging the concavity of CVaR constraints. C-ROVER has been evaluated through various simulations and real-world experiments, demonstrating its capability to ensure safe, scalable, and real-time swarm navigation in cluttered scenarios.

Target search and tracking (SAT) is a fundamental problem for various robotic applications such as search and rescue and environmental exploration. This article proposes an informative trajectory planning approach, namely, reusable belief tree search with sigma point-based mutual information reward approximation (ReSPIRe), for SAT in unknown cluttered environments under considerably inaccurate prior target information and a limited sensing field of view (FOV). We first develop a novel sigma point (SP)-based approximation approach to fast and accurately estimate mutual information (MI) reward under non-Gaussian belief distributions, utilizing informative sampling in state and observation spaces to mitigate the computational intractability of integral calculation. To tackle the significant uncertainty associated with inadequate prior target information, we propose the hierarchical particle structure in ReSPIRe, which not only extracts critical particles for global route guidance, but also adjusts the particle number adaptively for planning efficiency. Building upon the hierarchical structure, we develop the reusable belief tree search (RBTS) approach to build a policy tree for online trajectory planning under uncertainty, which reuses rollout evaluation to improve planning efficiency. Extensive simulations and real-world experiments demonstrate that ReSPIRe outperforms representative benchmark methods with smaller MI approximation error, higher search efficiency, and more stable tracking performance, while maintaining outstanding computational efficiency.

Motion control of autonomous vehicles (AVs) that considers nonlinear dynamics and multidimensional motion coupling characteristics represents a critical research direction, particularly for vehicles operating under extreme conditions. However, the nonlinearity of model and the time-varying characteristics of reference states make control law design challenging, often resorting to computationally expensive approaches such as model predictive control (MPC). This paper proposes an explicit nonlinear control design framework for optimal trajectory tracking of AVs. Specifically, taking the three-degree-of-freedom vehicle dynamics model as an example, we first augment the system state to yield an affine representation, followed by input-output feedback linearization. The discrete-time linearized system is then augmented with reference states from the preview horizon, and a linear quadratic regulator is designed to provide an analytical solution for the virtual control inputs. Finally, the original control inputs are computed using the feedback linearization control law. We performed simulations to evaluate the effectiveness of our proposed approach and compared it against MPC. Results indicate that our proposed approach achieves tracking accuracy, smoothness, and robustness comparable to MPC, while significantly reducing computational requirements, with a computing time of nearly 0 ms.

Estimating hidden states in dynamical systems, also known as optimal filtering, is a long-standing problem in various fields of science and engineering. In this paper, we introduce a general filtering framework, $\textbf{LLM-Filter}$, which leverages large language models (LLMs) for state estimation by embedding noisy observations with text prototypes. In a number of experiments for classical dynamical systems, we find that first, state estimation can significantly benefit from the knowledge embedded in pre-trained LLMs. By achieving proper modality alignment with the frozen LLM, LLM-Filter outperforms the state-of-the-art learning-based approaches. Second, we carefully design the prompt structure, System-as-Prompt (SaP), incorporating task instructions that enable LLMs to understand tasks and adapt to specific systems. Guided by these prompts, LLM-Filter exhibits exceptional generalization, capable of performing filtering tasks accurately in changed or even unseen environments. We further observe a scaling-law behavior in LLM-Filter, where accuracy improves with larger model sizes and longer training times. These findings make LLM-Filter a promising foundation model of filtering.

Reinforcement learning (RL) has shown remarkable success in solving complex decision-making and control tasks. However, many model-free RL algorithms experience performance degradation due to inaccurate value estimation, particularly the overestimation of Q-values, which can lead to suboptimal policies. To address this issue, we previously proposed the Distributional Soft Actor-Critic (DSAC or DSACv1), an off-policy RL algorithm that enhances value estimation accuracy by learning a continuous Gaussian value distribution. Despite its effectiveness, DSACv1 faces challenges such as training instability and sensitivity to reward scaling, caused by high variance in critic gradients due to return randomness. In this paper, we introduce three key refinements to DSACv1 to overcome these limitations and further improve Q-value estimation accuracy: expected value substitution, twin value distribution learning, and variance-based critic gradient adjustment. The enhanced algorithm, termed DSAC with Three refinements (DSAC-T or DSACv2), is systematically evaluated across a diverse set of benchmark tasks. Without the need for task-specific hyperparameter tuning, DSAC-T consistently matches or outperforms leading model-free RL algorithms, including SAC, TD3, DDPG, TRPO, and PPO, in all tested environments. Additionally, DSAC-T ensures a stable learning process and maintains robust performance across varying reward scales. Its effectiveness is further demonstrated through real-world application in controlling a wheeled robot, highlighting its potential for deployment in practical robotic tasks.

In the realm of household robotics, the Zero-Shot Object Navigation (ZSON) task empowers agents to adeptly traverse unfamiliar environments and locate objects from novel categories without prior explicit training. This paper introduces VoroNav, a novel semantic exploration framework that proposes the Reduced Voronoi Graph to extract exploratory paths and planning nodes from a semantic map constructed in real time. By harnessing topological and semantic information, VoroNav designs text-based descriptions of paths and images that are readily interpretable by a large language model (LLM). In particular, our approach presents a synergy of path and farsight descriptions to represent the environmental context, enabling LLM to apply commonsense reasoning to ascertain waypoints for navigation. Extensive evaluation on HM3D and HSSD validates VoroNav surpasses existing benchmarks in both success rate and exploration efficiency (absolute improvement: +2.8% Success and +3.7% SPL on HM3D, +2.6% Success and +3.8% SPL on HSSD). Additionally introduced metrics that evaluate obstacle avoidance proficiency and perceptual efficiency further corroborate the enhancements achieved by our method in ZSON planning. Project page: https://voro-nav.github.io

This paper proposes an informative trajectory planning approach, namely, adaptive particle filter tree with sigma point-based mutual information reward approximation (ASPIRe), for mobile target search and tracking (SAT) in cluttered environments with limited sensing field of view. We develop a novel sigma point-based approximation to accurately estimate mutual information (MI) for general, non-Gaussian distributions utilizing particle representation of the belief state, while simultaneously maintaining high computational efficiency. Building upon the MI approximation, we develop the Adaptive Particle Filter Tree (APFT) approach with MI as the reward, which features belief state tree nodes for informative trajectory planning in continuous state and measurement spaces. An adaptive criterion is proposed in APFT to adjust the planning horizon based on the expected information gain. Simulations and physical experiments demonstrate that ASPIRe achieves real-time computation and outperforms benchmark methods in terms of both search efficiency and estimation accuracy.

Large-scale swarm robotic systems consisting of numerous cooperative agents show considerable promise for performing autonomous tasks across various sectors. Nonetheless, traditional motion planning approaches often face a trade-off between scalability and solution quality due to the exponential growth of the joint state space of robots. In response, this work proposes SwarmPRM, a hierarchical, scalable, computationally efficient, and risk-aware sampling-based motion planning approach for large-scale swarm robots. SwarmPRM utilizes a Gaussian Mixture Model (GMM) to represent the swarm’s macroscopic state and constructs a Probabilistic Roadmap in Gaussian space, referred to as the Gaussian roadmap, to generate a transport trajectory of GMM. This trajectory is then followed by each robot at the microscopic stage. To enhance trajectory safety, SwarmPRM incorporates the conditional value-at-risk (CVaR) in the collision checking process to impart the property of risk awareness to the constructed Gaussian roadmap. SwarmPRM then crafts a linear programming formulation to compute the optimal GMM transport trajectory within this roadmap. Extensive simulations demonstrate that SwarmPRM outperforms state-of-the-art methods in computational efficiency, scalability, and trajectory quality while offering the capability to adjust the risk tolerance of generated trajectories.

Swarm robotics has garnered significant attention due to its ability to accomplish elaborate and synchronized tasks. Existing methodologies for motion planning of swarm robotic systems mainly encounter difficulties in scalability and safety guarantee. To address these limitations, we propose a Risk-aware swarm mOtion planner using conditional ValuE-at-Risk (ROVER) that systematically navigates large-scale swarms through cluttered environments while ensuring safety. ROVER formulates a finite-time model predictive control (FTMPC) problem predicated upon the macroscopic state of the robot swarm represented by a Gaussian Mixture Model (GMM) and integrates conditional value-at-risk (CVaR) to ensure collision avoidance. The key component of ROVER is imposing a CVaR constraint on the distribution of the Signed Distance Function between the swarm GMM and obstacles in the FTMPC to enforce collision avoidance. Utilizing the analytical expression of CVaR of a GMM derived in this work, we develop a computationally efficient solution to solve the non-linear constrained FTMPC through sequential linear programming. Simulations and comparisons with representative benchmark approaches demonstrate the effectiveness of ROVER in flexibility, scalability, and safety guarantee.

Real-time safety-critical control is essential for high-level autonomous driving. Existing methods usually formulate safety-critical control as a constrained optimal control problem (COCP), and suffer from high computational complexity of the underlying iterative optimization processes. To address the issue of complexity, this paper presents an explicit safety-critical control method called the Control Safety Function (CSF) approach, which can replace online optimization with an analytical control law, dramatically enhancing real-time control capabilities. The CSF is formulated as the weighted sum of Control Lyapunov Function (CLF) and Control Barrier Functions (CBFs), with the value of CSF increasing to infinity as the state approaches the boundary of a safe set. The explicit control law is then derived from the gradient of CSF and system dynamics. Different from existing explicit controllers that can only apply to systems of relative degree one, the CSF method provides an approach to enforce safety constraints to systems with high relative degree, making CSF especially suitable for autonomous driving. The CSF approach is evaluated in a vehicle path-tracking scenario with multiple obstacles, accompanied by a comparative analysis against the Model Predictive Control (MPC) method. Simulation results indicate that CSF achieves control accuracy comparable to MPC, with significant reduction in computation time - approximately 3.23 ms per step, which is about 94.0% faster. These results suggest that CSF is a promising approach for real-time safety-critical control of high-level autonomous driving.

Reinforcement learning (RL) has gained significant interest for its potential to improve decision and control in autonomous driving. However, current approaches have yet to demonstrate sufficient scenario generality and observation generality, hindering their wider utilization. To address these limitations, we propose a unified benchmark simulator for RL algorithms (called IDSim) to facilitate decision and control for high-level autonomous driving, with emphasis on diverse scenarios and a unified observation interface. IDSim is composed of a scenario library and a simulation engine, and is designed with execution efficiency and determinism in mind. The scenario library covers common urban scenarios, with automated random generation of road structure and traffic flow, and the simulation engine operates on the generated scenarios with dynamic interaction support. We conduct four groups of benchmark experiments with five common RL algorithms and focus on challenging signalized intersection scenarios with varying conditions. The results showcase the reliability of the simulator and reveal its potential to improve the generality of RL algorithms. Our analysis suggests that multi-task learning and observation design are potential areas for further algorithm improvement.

Bayesian nonparametric models, such as the Dirichlet Process Gaussian Process (DPGP), have been shown very effective at learning models of dynamic targets exclusively from data. Previous work on batch DPGP learning and inference, however, ceases to be efficient in multi-sensor applications that require decentralized measurements to be obtained sequentially over time. Batch processing, in this case, leads to redundant computations that may hinder online applicability. This paper develops a recursive approach for DPGP learning and inference in which a novel Dirichlet Process prior based on Wasserstein metric is used for measuring the similarity between multiple Gaussian Processes (GPs). Combined with the GP recursive fusion law, the proposed recursive DPGP fusion approach enables efficient online data fusion. The problem of active sensing for recursive DPGP learning and inference is also investigated by uncertainty reduction via expected mutual information. Simulation and experimental results show that the proposed approach successfully learns the models of moving targets and outperforms existing benchmark methods.

Deep learning techniques have been widely used in autonomous driving community for the purpose of environment perception. Recently, it starts being adopted for learning end-to-end controllers for complex driving scenarios. However, the complexity and nonlinearity of the network architecture limits its interpretability to understand driving scenarios and judge the importance of certain visual regions in sensory scenes. In this paper, based on the convolutional neural network (CNN), we propose two complementary frameworks to automatically determine the most contributive regions of the input scenes, offering intuitive knowledge of how a trained end-to-end autonomous vehicle controller understands driving scenarios. In the first framework, a feature map-based method is proposed by leveraging current progress in CNN visualization, in which the deconvolution approach recovers the feature maps to extract features that contribute most to understand driving scenes. In the second framework, the importance level of regions is ranked using the error map between the labeled and predicted control inputs generated by occluding different parts of input scenes, thus providing a pixel-wise rank of importance. Test data sets with extracted contributive regions are input to the CNN controller. Then, different CNN controllers trained with the new data sets preprocessed using our proposed frameworks are verified via closed-loop tests. Results show that both the features identified from the first framework and the regions identified from the second framework are of crucial importance to scene understanding for the controller and can significantly affect the performance of CNN controllers.

Deep learning-based approaches have been widely used for training controllers for autonomous vehicles due to their powerful ability to approximate nonlinear functions or policies. However, the training process usually requires large labeled data sets and takes a lot of time. In this paper, we analyze the influences of features on the performance of controllers trained using the convolutional neural networks (CNNs), which gives a guideline of feature selection to reduce computation cost. We collect a large set of data using The Open Racing Car Simulator (TORCS) and classify the image features into three categories (sky-related, roadside-related, and road-related features). We then design two experimental frameworks to investigate the importance of each single feature for training a CNN controller. The first framework uses the training data with all three features included to train a controller, which is then tested with data that has one feature removed to evaluate the feature's effects. The second framework is trained with the data that has one feature excluded, while all three features are included in the test data. Different driving scenarios are selected to test and analyze the trained controllers using the two experimental frameworks. The experiment results show that (1) the road-related features are indispensable for training the controller, (2) the roadside-related features are useful to improve the generalizability of the controller to scenarios with complicated roadside information, and (3) the sky-related features have limited contribution to train an end-to-end autonomous vehicle controller.

We propose in this paper an autonomous motion planning framework for companion robots to accompany humans in a socially desirable manner, which takes safety and comfort requirements into account. The overall framework consists of two parts: first, a novel parallel interacting multiple model-unscented Kalman filter (PIMM-UKF) approach is developed to simultaneously estimate human motion states and model mismatch, and then systematically predict the position and velocity of the human for a finite horizon. Second, based on the predicted human states, a nonlinear model predictive control (MPC) technique is utilized for the robot motion planning. The simulation results have demonstrated the superior performance in prediction using the PIMM-UKF approach. The effectiveness of the MPC planner is also shown by successfully facilitating the socially desirable companion behavior.

To improve vehicle path-following performance and to reduce driver workload, a human-centered feed-forward control (HCFC) system for a vehicle steering system is proposed. To be specific, a novel dynamic control strategy for the steering ratio of vehicle steering systems that treats vehicle speed, lateral deviation, yaw error, and steering angle as the inputs and a driver's expected steering ratio as the output is developed. To determine the parameters of the proposed dynamic control strategy, drivers are classified into three types according to the level of sensitivity to errors, i.e., low, middle, and high. The proposed HCFC system offers a human-centered steering system (HCSS) with a tunable steering gain, which can assist drivers in tracking a given path with smaller steering wheel angles and change rate of the angle by adaptively adjusting steering ratio according to driver's path-following characteristics, reducing the driver's workload. A series of experiments of tracking the centerline of double lane change (DLC) are conducted in CarSim and three different types of drivers are subsequently selected to test in a portable driving simulator under a fixed-speed condition. The simulation and experiment results show that the proposed HCSS with the dynamic control strategy, as compared with the classical control strategy of steering ratio, can improve task performance by about 7% and reduce the driver's physical workload and mental workload by about 35% and 50%, respectively, when following the given path.

Accurate detection and tracking of traffic participants are crucial to advanced driver assistance systems. This paper presents a centralized object tracking approach for surrounding objects in road traffic environments by using multiple linearly arrayed ultrasonic sensors. An ultrasonic sensor model is specifically developed for traffic environment, which consists of detection scope, chance of detection and ranging error, incorporating factors of object shapes, materials, distances and orientations. A centralized filter is designed to selectively fuse new measurements that are obtained using the Extended Kalman Filter (EKF) from certain sensors to conduct object tracking at each step. The effectiveness of proposed method is validated by simulations, which is found to have superior tracking performance compared to traditional triangle localization method, with more stable and smaller tracking error, especially when the object is entering or leaving the detection area.

In this paper, an optimization method of gear shift schedule for electric buses equipped with 4-AMT is proposed based on Dynamic Programming (DP) to improve the energy economy of the vehicle. A gear shift schedule that can be used in real-vehicle is extracted based on analysis of the obtained optimal gear shift points by DP approach in Chinese typical urban driving-cycles. Compared to the traditional two-parameter gear shift schedule in both simulation and real vehicle platform, the extracted gear shift schedule is proved to improve the energy economy of the electric vehicles (EVs) obviously.