Hierarchical Motion Planning for Autonomous Vehicles in Unstructured Dynamic Environments

This paper presents a hierarchical motion planner for generating smooth and feasible trajectories for autonomous vehicles in unstructured environments with static and moving obstacles. The framework enables real-time computation by progressively shrinking the solution space. First, a graph searcher based on combined heuristic and partial motion planning is proposed for finding coarse trajectories in spa- tiotemporal space. To enable fast online planning, a time interval-based algorithm that considers obstacle prediction trajectories is proposed, which uses line segment intersection detection to check for collisions. Second, to practically smooth the coarse trajectory, a continuous optimizer is implemented in three layers, corresponding to the whole path, the near- future path and the speed profile. We use discrete points to represent the far-future path and parametric curves to represent the near-future path and the whole speed profile. The approach is validated in both simulations and real-world off- road environments based on representative scenarios, including the “wait and go” scenario. The experimental results show that the proposed method improves the success rate and travel efficiency while actively avoiding static and moving obstacles.