Abstract: In this letter, we propose a bi-stage optimal trajectory planning method to address the challenges of planning in unstructured environments. In the first stage, we leverage the incremental ...