Hierarchical Motion Planning for Autonomous Vehicles in Unstructured Dynamic Environments

This letter 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 spatiotemporal 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.