Online Trajectory Replanning for Sudden Environmental Changes During Automated Parking: A Parallel Stitching Method

Trajectory planning for automated parking has been widely known as more challenging than that for on-road driving due to the nonconvex kinematics, high-dimensional collision-avoidance constraints, and difficulty to determine the global optimum among many local optima. Changes in a dynamic environment easily make a previously planned parking trajectory invalid. Compared with the on-road planners which evade newly emerged obstacles via a nudge or side pass, a parking planner has to replan a completely different trajectory for evasion. A qualified online trajectory replanner should run fast, ensure trajectory continuity, and avoid extra halfway stops. This paper proposes a parallel stitching strategy to fulfill these demands. When the ego vehicle tracks an originally planned parking trajectory online, an evasive trajectory begins to be replanned once a new obstacle is found to block the way. Thereafter, a connective trajectory is planned, the two ends of which are future poses along the original trajectory and the evasive trajectory, respectively. The two ends of the connective trajectory are chosen by greedily evaluating various candidates via parallel computation, which ensures that our replanner runs fast with high solution quality. According to our real-world experiments and simulations, the proposed replanner outperforms the existing ones w.r.t. solution speed and quality. As an interesting feature, our proposed replanner also supports switching to a better homotopy class online.