We study the sparsity and optimality properties of crowd navigation and find that existing techniques do not satisfy both criteria simultaneously: either they achieve optimality with a prohibitive number of samples or tractability assumptions make them fragile to catastrophe. For example, if the human and robot are modeled independently, then tractability is attained but the planner is prone to overcautious or overaggressive behavior. For sampling based motion planning of joint human-robot cost functions, for @math agents and @math step lookahead, @math samples are needed for coverage of the action space. Advanced approaches statically partition the action space into free-space and then sample in those convex regions. However, if the human is into free-space, then the partition is misleading and sampling is unsafe: free space will soon be occupied. We diagnose the cause of these deficiencies---optimization happens over space---and propose a novel solution: optimize over trajectory space by using a Gaussian process (GP) basis. We exploit the "kernel trick" of GPs, where a continuum of trajectories are captured with a mean and covariance function. By using the mean and covariance as proxies for a trajectory family we reason about collective trajectory behavior without resorting to sampling. The GP basis is sparse and optimal with respect to collision avoidance and robot and crowd intention and flexibility. GP sparsity leans heavily on the insight that joint action space decomposes into free regions; however, the decomposition contains feasible solutions only if the partition is dynamically generated. We call our approach .
@cite_30: 
@cite_38: Vehicle navigation in dynamic environments is an important challenge, especially when the motion of the objects populating the environment is unknown. Traditional motion planning approaches are too slow to be applied in real-time to this domain, hence, new techniques are needed. Recently, iterative planning has emerged as a promising approach. Nevertheless, existing iterative methods do not provide a way to estimating the future behaviour of moving obstacles and to use the resulting estimates in trajectory computation. This paper presents an iterative planning approach that addresses these two issues. It consists of two complementary methods: 1) A motion prediction method which learns typical behaviours of objects in a given environment. 2) An iterative motion planning technique based on the concept of Velocity Obstacles.
@cite_37: In this paper, we study the safe navigation of a mobile robot through crowds of dynamic agents with uncertain trajectories. Existing algorithms suffer from the “freezing robot” problem: once the environment surpasses a certain level of complexity, the planner decides that all forward paths are unsafe, and the robot freezes in place (or performs unnecessary maneuvers) to avoid collisions. Since a feasible path typically exists, this behavior is suboptimal. Existing approaches have focused on reducing the predictive uncertainty for individual agents by employing more informed models or heuristically limiting the predictive covariance to prevent this overcautious behavior. In this work, we demonstrate that both the individual prediction and the predictive uncertainty have little to do with the frozen robot problem. Our key insight is that dynamic agents solve the frozen robot problem by engaging in “joint collision avoidance”: They cooperatively make room to create feasible trajectories. We develop IGP, a nonparametric statistical model based on dependent output Gaussian processes that can estimate crowd interaction from data. Our model naturally captures the non-Markov nature of agent trajectories, as well as their goal-driven navigation. We then show how planning in this model can be efficiently implemented using particle based inference. Lastly, we evaluate our model on a dataset of pedestrians entering and leaving a building, first comparing the model with actual pedestrians, and find that the algorithm either outperforms human pedestrians or performs very similarly to the pedestrians. We also present an experiment where a covariance reduction method results in highly overcautious behavior, while our model performs desirably.
@cite_64: Whenever people move through their environments they do not move randomly. Instead, they usually follow specific trajectories or motion patterns corresponding to their intentions. Knowledge about such patterns enables a mobile robot to robustly keep track of persons in its environment and to improve its behavior. In this paper we propose a technique for learning collections of trajectories that characterize typical motion patterns of persons. Data recorded with laser-range finders are clustered using the expectation maximization algorithm. Based on the result of the clustering process, we derive a hidden Markov model that is applied to estimate the current and future positions of persons based on sensory input. We also describe how to incorporate the probabilistic belief about the potential trajectories of persons into the path planning process of a mobile robot. We present several experiments carried out in different environments with a mobile robot equipped with a laser-range scanner and a camera system...
@cite_4: Abstract This paper considers the decision-making problem for a vehicle crossing a road intersection in the presence of other, potentially errant, drivers. This problem is considered in a game-theoretic framework, where the errant drivers are assumed to be capable of causing intentional collisions. Our approach is to simulate the possible behaviors of errant drivers using RRT-Reach, a modified application of rapidly-exploring random trees. A novelty in RRT-Reach is the use of a dual exploration-pursuit mode, which allows for efficient approximation of the errant reachability set for some fixed time horizon. Through simulation and experimental results with a small autonomous vehicle, we demonstrate that this threat assessment algorithm can be used in real-time to minimize the risk of collision.
@cite_60: More than 15 years after model predictive control (MPC) appeared in industry as an effective means to deal with multivariable constrained control problems, a theoretical basis for this technique has started to emerge. The issues of feasibility of the on-line optimization, stability and performance are largely understood for systems described by linear models. Much progress has been made on these issues for non-linear systems but for practical applications many questions remain, including the reliability and efficiency of the on-line computation scheme. To deal with model uncertainty ‘rigorously’ an involved dynamic programming problem must be solved. The approximation techniques proposed for this purpose are largely at a conceptual stage. Among the broader research needs the following areas are identified: multivariable system identification, performance monitoring and diagnostics, non-linear state estimation, and batch system control. Many practical problems like control objective prioritization and symptom-aided diagnosis can be integrated systematically and effectively into the MPC framework by expanding the problem formulation to include integer variables yielding a mixed-integer quadratic or linear program. Efficient techniques for solving these problems are becoming available.
@cite_29: This paper presents the first randomized approach to kinodynamic planning (also known as trajectory planning or trajectory design). The task is to determine control inputs to drive a robot from an ini ial configuration and velocity to a goal configuration and velocity while obeying physically based dynamical models and avoiding obstacles in the robot’s environment. The authors consider generic systems that express the nonlinear dynamics of a robot in terms of the robot’s high-dimensional configuration space. Kinodynamic planning is treated as a motion-planning problem in a higher dimensional state space that has both first-order differential constraints and obstacle-based global constraints. The state space serves the same role as the configuration space for basic path planning; however, standard randomized path-planning techniques do not directly apply to planning trajectories in the state space. The authors have developed a randomized planning approach that is particularly tailored to trajectory plannin...
@cite_6: 
@cite_56: Obstacle avoidance, and by extension collision checking, is a basic requirement for robot autonomy. Most classical approaches to collision-checking ignore the uncertainties associated with the robot and obstacle's geometry and position. It is natural to use a probabilistic description of the uncertainties. However, constraint satisfaction cannot be guaranteed, in this case, and collision constraints must instead be converted to chance constraints. Standard results for linear probabilistic constraint evaluation have been applied to probabilistic collision evaluation; however, this approach ignores the uncertainty associated with the sensed obstacle. An alternative formulation of probabilistic collision checking that accounts for robot and obstacle uncertainty is presented which allows for dependent object distributions (e.g., interactive robot-obstacle models). In order to efficiently enforce the resulting collision chance constraints, an approximation is proposed and the validity of this approximation is evaluated. The results presented here have been applied to robot-motion planning in dynamic, uncertain environments.
@cite_3: 
@cite_63: In order to effectively plan paths in environments inhabited by humans, robots must accurately predict human motion. Typical approaches to human prediction simply assume a constant velocity which is not always valid. This paper proposes to determine the likely navigation intent of humans and use that to predict human motion. Navigation intent is determined by the function and structure of the environment. Manually assigned functional places are combined with automatically extracted navigation way-points to define a number of likely navigation targets within the environment. To predict human motion toward these targets, a probabilistic model of human motion is proposed which is based on motion probability grids generated from observed motion. The models of human navigation intent and motion are integrated with an autonomous mobile robot system, with a laser range sensor detecting humans moving within the environment, and a path planning system. The models of human navigation intent and motion are verified using real captured human motion data from an office environment. Examples of human motion prediction are also presented.
@cite_46: 
@cite_10: This paper presents a strategy for planning robot motions in dynamic, uncertain environments (DUEs). Successful and efficient robot operation in such environments requires reasoning about the future evolution and uncertainties of the states of the moving agents and obstacles. A novel procedure to account for future information gathering (and the quality of that information) in the planning process is presented. To approximately solve the stochastic dynamic programming problem that is associated with DUE planning, we present a partially closed-loop receding horizon control algorithm whose solution integrates prediction, estimation, and planning while also accounting for chance constraints that arise from the uncertain locations of the robot and obstacles. Simulation results in simple static and dynamic scenarios illustrate the benefit of the algorithm over classical approaches. The approach is also applied to more complicated scenarios, including agents with complex, multimodal behaviors, basic robot-agent interaction, and agent information gathering.
@cite_66: Autonomous vehicles require optimal path planning algorithms to achieve mission goals while avoiding obstacles and being robust to uncertainties. The uncertainties arise from exogenous disturbances, modeling errors, and sensor noise, which can be characterized via stochastic models. Previous work dened a notion of robustness in a stochastic setting by using the concept of chance constraints. This requires that mission constraint violation can occur with a probability less than a prescribed value. In this paper we describe a novel method for optimal chance constrained path planning with feedback design. The approach optimizes both the reference trajectory to be followed and the feedback controller used to reject uncertainty. Our method extends recent results in constrained control synthesis based on convex optimization to solve control problems with nonconvex constraints. This extension is essential for path planning problems, which inherently have nonconvex obstacle avoidance constraints. Unlike previous approaches to chance constrained path planning, the new approach optimizes the feedback gain as well as the reference trajectory. The key idea is to couple a fast, nonconvex solver that does not take into account uncertainty, with existing robust approaches that apply only to convex feasible regions. By alternating between robust and nonrobust solutions, the new algorithm guarantees convergence to a global optimum. We apply the new method to an unmanned aircraft and show simulation results that demonstrate the ecacy of the approach.
@cite_20: Autonomous vehicles need to plan trajectories to a specified goal that avoid obstacles. Previous approaches that used a constrained optimization approach to solve for finite sequences of optimal control inputs have been highly effective. For robust execution, it is essential to take into account the inherent uncertainty in the problem, which arises due to uncertain localization, modeling errors, and disturbances. Prior work has handled the case of deterministically bounded uncertainty. We present here an alternative approach that uses a probabilistic representation of uncertainty, and plans the future probabilistic distribution of the vehicle state so that the probability of collision with obstacles is below a specified threshold. This approach has two main advantages; first, uncertainty is often modeled more naturally using a probabilistic representation (for example in the case of uncertain localization); second, by specifying the probability of successful execution, the desired level of conservatism in the plan can be specified in a meaningful manner. The key idea behind the approach is that the probabilistic obstacle avoidance problem can be expressed as a disjunctive linear program using linear chance constraints. The resulting disjunctive linear program has the same complexity as that corresponding to the deterministic path planning problem with no representation of uncertainty. Hence the resulting problem can be solved using existing, efficient techniques, such that planning with uncertainty requires minimal additional computation. Finally, we present an empirical validation of the new method with a number of aircraft obstacle avoidance scenarios.
Independent agent Kalman filters are a common starting point for crowd prediction. However, this approach leads to an uncertainty explosion that makes efficient navigation impossible @cite_37 . Some research has thus focused on uncertainty. For instance @cite_63 @cite_64 @cite_38 develop high fidelity independent models, in the hope that controlled uncertainty will improve navigation performance. @cite_10 predictive uncertainty is bounded; intractability is avoided with receding horizon control @cite_60 ; collision checking algorithms developed in @cite_56 , with roots in @cite_66 @cite_6 , keeps navigation safe. The insight is that since replanning is used, predictive covariance can be held constant at measurement noise. @cite_20 @cite_3 more sophisticated individual models are developed: GP mixtures @cite_30 with a Dirichlet Process (DP) prior over mixture weights @cite_46 . DP priors allow the number of motion patterns to be data driven while the GP enables inter-motion variability; RRT @cite_29 finds feasible paths; collision checking @cite_4 guarantees safety. In the work above, independent agent models are the core contribution; we show that this is insufficient for crowd navigation.