Efficient planning in dynamic and uncertain environments is a fundamental challenge in robotics. In the context of trajectory optimization, the feasibility of paths can change as the environment evolves. Therefore, it can be beneficial to reason about multiple possible paths simultaneously. We build on prior work that considers graph-based trajectories to find solutions in multiple homotopy classes concurrently. Specifically, we extend this previous work to an online setting where the unreachable (in time) part of the graph is pruned and the remaining graph is reoptimized at every time step. As the robot moves within the graph on the path that is most promising, the pruning and reoptimization allows us to retain candidate paths that may become more viable in the future as the environment changes, essentially enabling the robot to dynamically switch between numerous homotopy classes. We compare our approach against prior work without the homotopy switching capability and show improved performance across several metrics in simulation with a 2D robot in multiple dynamic environments under noisy measurements and execution.
@cite_10: We present ( RRT^ X ), the first asymptotically optimal sampling-based motion planning algorithm for real-time navigation in dynamic environments (containing obstacles that unpredictably appear, disappear, and move). Whenever obstacle changes are observed, e.g., by onboard sensors, a graph rewiring cascade quickly updates the search-graph and repairs its shortest-path -to-goal subtree. Both graph and tree are built directly in the robot’s state space, respect the kinematics of the robot, and continue to improve during navigation. ( RRT^ X ) is also competitive in static environments—where it has the same amortized per iteration runtime as RRT and RRT* ( ( n ) ) and is faster than RRT# ( ( ^2 n ) ). In order to achieve (O ( n ) ) iteration time, each node maintains a set of (O ( n ) ) expected neighbors, and the search graph maintains ( )-consistency for a predefined ( ).
@cite_1: We present a novel optimization-based algorithm for motion planning in dynamic environments. Our approach uses a stochastic trajectory optimization framework to avoid collisions and satisfy smoothness and dynamics constraints. Our algorithm does not require a priori knowledge about global motion or trajectories of dynamic obstacles. Rather, we compute a conservative local bound on the position or trajectory of each obstacle over a short time and use the bound to compute a collision-free trajectory for the robot in an incremental manner. Moreover, we interleave planning and execution of the robot in an adaptive manner to balance between the planning horizon and responsiveness to obstacle. We highlight the performance of our planner in a simulated dynamic environment with the 7-DOF PR2 robot arm and dynamic obstacles.
@cite_15: Incremental search algorithms, such as Generalized Fringe-Retrieving A* and D* Lite, reuse search trees from previous searches to speed up the current search and thus often find cost-minimal paths for series of similar search problems faster than by solving each search problem from scratch. However, existing incremental search algorithms have limitations. For example, D* Lite is slow on moving target search problems, where both the start and goal states can change over time. In this paper, we therefore introduce Moving Target D* Lite, an extension of D* Lite that uses the principle behind Generalized Fringe-Retrieving A* to repeatedly calculate a cost-minimal path from the hunter to the target in environments whose blockages can change over time. We demonstrate experimentally that Moving Target D* Lite is four to five times faster than Generalized Adaptive A*, which so far was believed to be the fastest incremental search algorithm for solving moving target search problems in dynamic environments.
@cite_13: Heuristic search methods promise to find shortest paths for path-planning problems faster than uninformed search methods. Incremental search methods, on the other hand, promise to find shortest paths for series of similar path-planning problems faster than is possible by solving each path-planning problem from scratch. In this article, we develop Lifelong Planning A* (LPA*), an incremental version of A* that combines ideas from the artificial intelligence and the algorithms literature. It repeatedly finds shortest paths from a given start vertex to a given goal vertex while the edge costs of a graph change or vertices are added or deleted. Its first search is the same as that of a version of A* that breaks ties in favor of vertices with smaller g-values but many of the subsequent searches are potentially faster because it reuses those parts of the previous search tree that are identical to the new one. We present analytical results that demonstrate its similarity to A* and experimental results that demonstrate its potential advantage in two different domains if the path-planning problems change only slightly and the changes are close to the goal.
@cite_11: This paper introduces a novel and general real-time adaptive motion planning (RAMP) approach suitable for planning trajectories of high-DOF or redundant robots, such as mobile manipulators, in dynamic environments with moving obstacles of unknown trajectories. The RAMP approach enables simultaneous path and trajectory planning and simultaneous planning and execution of motion in real time. It facilitates real-time optimization of trajectories under various optimization criteria, such as minimizing energy and time and maximizing manipulability. It also accommodates partially specified task goals of robots easily. The approach exploits redundancy in redundant robots (such as locomotion versus manipulation in a mobile manipulator) through loose coupling of robot configuration variables to best achieve obstacle avoidance and optimization objectives. The RAMP approach has been implemented and tested in simulation over a diverse set of task environments, including environments with multiple mobile manipulators. The results (and also the accompanying video) show that the RAMP planner, with its high efficiency and flexibility, not only handles a single mobile manipulator well in dynamic environments with various obstacles of unknown motions in addition to static obstacles, but can also readily and effectively plan motions for each mobile manipulator in an environment shared by multiple mobile manipulators and other moving obstacles.
While these planning approaches are typically employed in static environments, a number of algorithms @cite_1 have been proposed to handle navigating dynamic environments. Lifelong Planning A* @cite_13 , D* lite @cite_15 and comparable online planning algorithms outperform repeatedly replanning from scratch by incrementally finding shortest paths on a graph with changing edge costs. RRT-X @cite_10 an extension of RRT is suited to environments with moving obstacles and provides comparable runtime performance. RAMP @cite_11 draws from evolutionary computation by maintaining a population of trajectories and evaluating their respective quality (fitness) with respect to a cost function and has been shown to discover and exploit new homotopies. Since solutions can go in and out of feasibility as the environment changes access to solutions in multiple homotopy classes is beneficial. However, there exists a gap in current research with respect to solving the online planning problem by leveraging trajectories across different homotopy classes.