Trajectory optimization and motion planning for quadrotors in unstructured environments