Can fastrack be applied to autonomous driving, and how to modify it?

[dfridovi]

Yes, but (as with any serious problem) there will be some challenges.

• If you're using a serious, industrial dynamical model of the car, it probably has >6 states, which means you'll have to approximate the offline computation component of FaSTrack (there are some ways, e.g. here).
• The type of safety guarantee you'll get out of FaSTrack is highly dependent upon the type of planning algorithm you're using, and specifically if that algorithm is receding horizon or not. Receding horizon planning is generally not recursively feasible, regardless of any guarantees provided by FaSTrack, so if you care about viability, liveness, and recursive feasibility you'll need to solve those problems separately anyway. Also, the FaSTrack safety guarantee will depend on how you model other objects, especially moving objects like other cars and people, and whether or not that model is stochastic. For a good example of this, check out another recent project.

Just emphasizing this point again: FaSTrack gives you a safety margin around a planning algorithm that can be used for collision-checking during motion planning. It also gives you a safety controller that will definitely keep the vehicle within that safety margin. It works very well for low-dimensional (< 5) systems and has been demonstrated in slightly higher (7) dimensional systems. It does not tell you what planning algorithm to use, and it will not automatically guarantee safety if the environment is uncertain or the planning algorithm is not recursively feasible.

Hope this helps. I'm happy to discuss more.

[gilmoujw]

Hi David, based on your response I would like to ask a further question if possible.

From the paper 'Planning, Fast and Slow: A Framework for Adaptive Real-Time Safe Trajectory Planning', we have the quote "This provides autonomous systems with the ability to adapt motion plans to a priori unknown environments in real-time as sensor measurements detect new obstacles, and the flexibility to maneuver differently in the presence of obstacles than they would in free space, all while maintaining a strict safety guarantee."

Unless I am reading that wrong, shouldn't FaSTrack be applicable to receding horizon planning? Thank you for any clarification. I am currently looking into applying FaSTrack for an underwater robotics simulation application, which is why I ask.

[rorypeck]

Hi David, sorry to reply late, I am still working on the theory and algorithms, and not very sure about your opinions. As in the ans,

• Receding horizon planning is generally not recursively feasible
• will not automatically guarantee safety if the environment is uncertain or the planning algorithm is not recursively feasible.

that is to say, Receding horizon controller is not suitable to FaSTrack? If I choose the simple PID controller, can FaSTrack achieve good performance if applied into autonomous driving? And, I notice that the obstacles are represented by spheres, does it easy to represent them by boxes?

[dfridovi]

I think there are three issues in question here:

1. What's the deal with receding horizon planning?
2. Control vs. planning - what's the difference?
3. Obstacle modeling - are spheres important or do other models work?

Receding horizon planning

Ok so I think the receding horizon part needs some clarification. As I said before, in general, receding horizon planning is not recursively feasible although there are ways to make it so (e.g. this cool paper from U. Rosolia, a colleague of ours at Berkeley). In our work Planning, Fast and Slow, we indeed claim that our work provides a strict safety guarantee even when we replan. However, if you the proof of that claim in Section IV, you will see that we rely upon the specific geometric planning model used throughout the paper. Essentially, the proof boils down to the fact that the planner can always just decide to stop and hold position at any point in time; this implies recursive feasibility. Moreover, it can retrace any previous plan back to the starting point; this implies liveness. Of course, these properties do not hold for more general dynamical systems (e.g. any with inertia). The main idea behind FaSTrack (and Planning, Fast and Slow, for that matter) is still totally viable for such models; you just can't expect them to guarantee recursive feasibility if the underlying planning model is not recursively feasible.

To summarize: FaSTrack provides a low-level controller and a guaranteed tracking error bound that quantifies how far away the real system will ever get from a hypothetical dynamical model used for motion planning. This error bound can then be used by the motion planner to collision-check its trajectories. That's it. If the planner is recursively feasible, wonderful. If not, that's fine - using the tracking error bound is still (we would argue) a strictly better idea than not using it. Does this make sense?

Control vs. planning

@rorypeck, you're asking if PID control works with FaSTrack. The short version is that yes, you can totally use PID (and we do - we just call it LQR but LQR is just a fancy PD controller...) and wrap it with the safety controller from FaSTrack in a least restrictive control framework. I believe we explain this in the paper. However (as per this subheading title) I suspect there may be some misunderstanding underlying this question - let me emphasize: planning is not the same as control. In pretty much any robotics application, motion plans are generated (e.g. with an RRT, or iterative LQR, or some other technique) and then tracked or followed by a low-level controller of some kind. In FaSTrack, we provide a reachability-based low-level controller. The planner can be whatever algorithm you like, provided that the trajectories it outputs can be expressed as the solution to some ODE defined by a dynamical model. I hope this helps.

Obstacle representations

Obstacles can be totally generic. We used spheres because, in all honesty, it was the easiest thing I could think of that wasn't totally useless. Boxes are cool too. Occupancy grids. You name it. As long as you can do things like collision check everything should be fine.

[rorypeck]

Thanks for your patient explanations. So receding horizon planning is not suitable, but RRT and the like in OMPL which are recursively feasible and can be applied into fastrack, that is right?

[dfridovi]

I'm happy to explain - no worries.

Let me attempt to draw a distinction here between receding horizon / MPC planning and RRT-style approaches. In MPC, typically you plan for a short time horizon using some dynamical system model and design a sequence of controls whose corresponding state trajectory over that time horizon yields low cost; execute part of that trajectory; repeat. In RRT-style approaches you typically plan all the way to your end goal and just go - if you need to replan then you can, but that's not always necessary (e.g. if you know everything about the world already). RRT-style approaches are also typically, but not always, geometric or kinematic approaches meaning that the underlying dynamical model they assume is something like x_dot = u. In this case, they are always recursively feasible because setting u = 0 keeps the planner's state x constant. Recursive feasibility issues (in static environments as considered here) pop up only when it is impossible to maintain a constant state.

In our example in Planning, Fast and Slow, we used an geometric RRT to plan all the way to the goal and we replanned whenever we saw a new obstacle. This is recursively feasible because we can always set u = 0 for the planner and just hover inside the resulting tracking error bound.

However, we could equally well have used a more complicated kinodynamic planning model, either with RRT or MPC or what have you. We just would not inherit any recursive feasibility guarantees if those planners (with that model) did not have them to start with. Using FaSTrack in such situations is still a very good idea because it will still avoid any collisions due to errors tracking a planned trajectory. It just won't stop the planner from getting into situations where no safe plan exists.

I should also mention that several of us are working on an extension to FaSTrack that explicitly incorporates high-level logic to force receding horizon planners to be recursively feasible. Details will be forthcoming.

[gilmoujw]

Thanks David, your insights are very helpful. I have a question related to a couple of the last points you made, in part related to my specific application.

The idea would be to utilize D-lite path planning to plan a path in a partially unknown 3D underwater environment. D-lite is a geometric planner, like with the example used in Planning, Fast, and Slow. Obstacles would be augmented by the safety margin determined by FasTracks offline reachability analysis for a vehicle. Clearly there is no recursive feasibility in my case due to the vehicles inertia. If I used the hybrid tracking controller, as described in the original FasTrack paper, am I correct in saying that FasTrack can guarantee that the vehicle will not impact obstacles but it cannot guarantee that the planner will only put the vehicle in situations where it is still feasible to reach the goal location? The later restriction being due to the dynamic restrictions of the planning model.

Thank you for the help. I look forward to the extension you're talking about, it sounds interesting!

[dfridovi]

Aha so actually FaSTrack can do one better than that! Your planner (D*-lite) is geometric which means that it is actually recursively feasible. The fact that your actual vehicle model (in FaSTrack, the tracking model) has inertia will get accounted for by FaSTrack's tracking error bound. That's the beauty of FaSTrack! Bottom line is - you can get recursive feasibility by using a geometric planner and still guarantee collision avoidance by using FaSTrack to follow the geometric planner with the actual system. Does that make sense?

[gilmoujw]

That does make sense and is great to hear! I think I will move forward with trying to utilize FaSTrack for my application then. Thank you for all your help David