A Decoupling Principle for Simultaneous Localization and Planning Under Uncertainty in Multi-Agent Dynamic Environments

Simultaneous localization and planning for nonlinear stochastic systems under process and measurement uncertainties is a challenging problem. In its most general form, it is formulated as a stochastic optimal control problem in the space of feedback policies. The Hamilton-Jacobi-Bellman equation provides the theoretical solution of the optimal problem; but, as is typical of almost all nonlinear stochastic systems, optimally solving the problem is intractable. Moreover, even if an optimal solution was obtained, it would require centralized control, while multi-agent mobile robotic systems under dynamic environments require decentralized solutions. In this study, we aim for a theoretically sound solution for various modes of this problem, including the single-agent and multi-agent variations with perfect and imperfect state information, where the underlying state, control and observation spaces are continuous with discrete-time models. We introduce a decoupling principle for planning and control of multi-agent nonlinear stochastic systems based on a small noise asymptotics. Through this decoupling principle, under small noise, the design of the real-time feedback law can be decoupled from the off-line design of the nominal trajectory of the system. Further, for a multi-agent problem, the design of the feedback laws for different agents can be decoupled from each other, reducing the centralized problem to a decentralized problem requiring no communication during execution. The resulting solution is quantifiably near-optimal. We establish this result for all the above-mentioned variations, which results in the following variants: Trajectory-optimized Linear Quadratic Regulator (T-LQR), Multi-agent T-LQR (MT-LQR), Trajectory-optimized Linear Quadratic Gaussian (T-LQG), and Multi-agent T-LQG (MT-LQG). The decoupling principle provides the conditions under which a decentralized linear Gaussian system with a quadratic approximation of the cost, obtained by linearization around an optimally designed nominal trajectory can be utilized to control the nonlinear system. The resulting decentralized feedback solution at runtime, being decoupled with respect to the mobile agents, requires no communication between the agents during the execution phase. Moreover, the complexity of the solution vis-a-vis the computation of the nominal trajectory as well as the closed-loop gains is tractable with low polynomial orders of computation. Experimental implementation of the solution shows that the results hold for moderate levels of noise with high probability. Further optimizing the performance of this approach we show how to design a special cost function for the problem with imperfect state measurement that takes advantage of the fact that the estimation covariance of a linear Gaussian system is deterministic and not dependent on the observations. This design, which corresponds in our overall design to “belief space planning”, incorporates the consequently deterministic cost of the stochastic feedback system into the deterministic design of the nominal trajectory to obtain an optimal nominal trajectory with the best estimation performance. Then, it utilizes the T-LQG approach to design an optimal feedback law to track the designed nominal trajectory. This iterative approach can be used to further tune both the open loop as well as the decentralized feedback gain portions of the overall design. We also provide the multi-agent variant of this approach based on the MT-LQG method. Based on the near-optimality guarantees of the decoupling principle and the TLQG approach, we analyze the performance and correctness of a well-known heuristic in robotic path planning. We show that optimizing measures of the observability Gramian as a surrogate for estimation performance may provide irrelevant or misleading trajectories for planning under observation uncertainty. We then consider systems with non-Gaussian perturbations. An alternative heuristic method is proposed that aims for fast planning in belief space under non- Gaussian uncertainty. We provide a special design approach based on particle filters that results in a convex planning problem implemented via a model predictive control strategy in convex environments, and a locally convex problem in non-convex environments. The environment here refers to the complement of the region in Euclidean space that contains the obstacles or “no fly zones”. For non-convex dynamic environments, where the no-go regions change dynamically with time, we design a special form of an obstacle penalty function that incorporates non-convex time-varying constraints into the cost function, so that the decoupling principle still applies to these problems. However, similar to any constrained problem, the quality of the optimal nominal trajectory is dependent on the quality of the solution obtainable for the nonlinear optimization problem. We simulate our algorithms for each of the problems on various challenging situations, including for several nonlinear robotic models and common measurement models. In particular, we consider 2D and 3D dynamic environments for heterogeneous holonomic and non-holonomic robots, and range and bearing sensing models. Future research can potentially extend the results to more general situations including continuous-time models

A Decoupling Principle for Simultaneous Localization and Planning Under Uncertainty in Multi-Agent Dynamic Environments

Simultaneous localization and planning for nonlinear stochastic systems under process and measurement uncertainties is a challenging problem. In its most general form, it is formulated as a stochastic optimal control problem in the space of feedback policies. The Hamilton-Jacobi-Bellman equation provides the theoretical solution of the optimal problem; but, as is typical of almost all nonlinear stochastic systems, optimally solving the problem is intractable. Moreover, even if an optimal solution was obtained, it would require centralized control, while multi-agent mobile robotic systems under dynamic environments require decentralized solutions. In this study, we aim for a theoretically sound solution for various modes of this problem, including the single-agent and multi-agent variations with perfect and imperfect state information, where the underlying state, control and observation spaces are continuous with discrete-time models. We introduce a decoupling principle for planning and control of multi-agent nonlinear stochastic systems based on a small noise asymptotics. Through this decoupling principle, under small noise, the design of the real-time feedback law can be decoupled from the off-line design of the nominal trajectory of the system. Further, for a multi-agent problem, the design of the feedback laws for different agents can be decoupled from each other, reducing the centralized problem to a decentralized problem requiring no communication during execution. The resulting solution is quantifiably near-optimal. We establish this result for all the above-mentioned variations, which results in the following variants: Trajectory-optimized Linear Quadratic Regulator (T-LQR), Multi-agent T-LQR (MT-LQR), Trajectory-optimized Linear Quadratic Gaussian (T-LQG), and Multi-agent T-LQG (MT-LQG). The decoupling principle provides the conditions under which a decentralized linear Gaussian system with a quadratic approximation of the cost, obtained by linearization around an optimally designed nominal trajectory can be utilized to control the nonlinear system. The resulting decentralized feedback solution at runtime, being decoupled with respect to the mobile agents, requires no communication between the agents during the execution phase. Moreover, the complexity of the solution vis-a-vis the computation of the nominal trajectory as well as the closed-loop gains is tractable with low polynomial orders of computation. Experimental implementation of the solution shows that the results hold for moderate levels of noise with high probability. Further optimizing the performance of this approach we show how to design a special cost function for the problem with imperfect state measurement that takes advantage of the fact that the estimation covariance of a linear Gaussian system is deterministic and not dependent on the observations. This design, which corresponds in our overall design to “belief space planning”, incorporates the consequently deterministic cost of the stochastic feedback system into the deterministic design of the nominal trajectory to obtain an optimal nominal trajectory with the best estimation performance. Then, it utilizes the T-LQG approach to design an optimal feedback law to track the designed nominal trajectory. This iterative approach can be used to further tune both the open loop as well as the decentralized feedback gain portions of the overall design. We also provide the multi-agent variant of this approach based on the MT-LQG method. Based on the near-optimality guarantees of the decoupling principle and the TLQG approach, we analyze the performance and correctness of a well-known heuristic in robotic path planning. We show that optimizing measures of the observability Gramian as a surrogate for estimation performance may provide irrelevant or misleading trajectories for planning under observation uncertainty. We then consider systems with non-Gaussian perturbations. An alternative heuristic method is proposed that aims for fast planning in belief space under non- Gaussian uncertainty. We provide a special design approach based on particle filters that results in a convex planning problem implemented via a model predictive control strategy in convex environments, and a locally convex problem in non-convex environments. The environment here refers to the complement of the region in Euclidean space that contains the obstacles or “no fly zones”. For non-convex dynamic environments, where the no-go regions change dynamically with time, we design a special form of an obstacle penalty function that incorporates non-convex time-varying constraints into the cost function, so that the decoupling principle still applies to these problems. However, similar to any constrained problem, the quality of the optimal nominal trajectory is dependent on the quality of the solution obtainable for the nonlinear optimization problem. We simulate our algorithms for each of the problems on various challenging situations, including for several nonlinear robotic models and common measurement models. In particular, we consider 2D and 3D dynamic environments for heterogeneous holonomic and non-holonomic robots, and range and bearing sensing models. Future research can potentially extend the results to more general situations including continuous-time models

Planning, Estimation and Control for Mobile Robot Localization with Application to Long-Term Autonomy

There may arise two kinds of challenges in the problem of mobile robot localization; (i) a robot may have an a priori map of its environment, in which case the localization problem boils down to estimating the robot pose relative to a global frame or (ii) no a priori map information is given, in which case a robot may have to estimate a model of its environment and localize within it. In the case of a known map, simultaneous planning while localizing is a crucial ability for operating under uncertainty. We first address this problem by designing a method to dynamically replan while the localization uncertainty or environment map is updated. Extensive simulations are conducted to compare the proposed method with the performance of FIRM (Feedback-based Information RoadMap). However, a shortcoming of this method is its reliance on a Gaussian assumption for the Probability Density Function (pdf) on the robot state. This assumption may be violated during autonomous operation when a robot visits parts of the environment which appear similar to others. Such situations lead to ambiguity in data association between what is seen and the robot’s map leading to a non-Gaussian pdf on the robot state. We address this challenge by developing a motion planning method to resolve situations where ambiguous data associations result in a multimodal hypothesis on the robot state. A Receding Horizon approach is developed, to plan actions that sequentially disambiguate a multimodal belief to achieve tight localization on the correct pose in finite time. In our method, disambiguation is achieved through active data associations by picking target states in the map which allow distinctive information to be observed for each belief mode and creating local feedback controllers to visit the targets. Experiments are conducted for a kidnapped physical ground robot operating in an artificial maze-like environment. The hardest challenge arises when no a priori information is present. In longterm tasks where a robot must drive for long durations before closing loops, our goal is to minimize the localization error growth rate such that; (i) accurate data associations can be made for loop closure, or (ii) in cases where loop closure is not possible, the localization error stays limited within some desired bounds. We analyze this problem and show that accurate heading estimation is key to limiting localization error drift. We make three contributions in this domain. First we present a method for accurate long-term localization using absolute orientation measurements and analyze the underlying structure of the SLAM problem and how it is affected by unbiased heading measurements. We show that consistent estimates over a 100km trajectory are possible and that the error growth rate can be controlled with active data acquisition. Then we study the more general problem when orientation measurements may not be present and develop a SLAM technique to separate orientation and position estimation. We show that our method’s accuracy degrades gracefully compared to the standard non-linear optimization based SLAM approach and avoids catastrophic failures which may occur due a bad initial guess in non-linear optimization. Finally we take our understanding of orientation sensing into the physical world and demonstrate a 2D SLAM technique that leverages absolute orientation sensing based on naturally occurring structural cues. We demonstrate our method using both high-fidelity simulations and a real-world experiment in a 66, 000 square foot warehouse. Empirical studies show that maps generated by our approach never suffer catastrophic failure, whereas existing scan matching based SLAM methods fail ≈ 50% of the time