TauLabs/TauLabs — pull request #479 by kubark42 (2013-04-27 22:53:23+00:00)
Title: Path planner refactoring

This is a refactoring of our navigation approach. This splits navigation into three well-defined hierarchies (the fourth is missing as we do not yet have a Flight Director).

1) Path planner: takes the desired trajectory and continuously computes the required feedback control for a trajectory in Rn. In the simplest case, the path planner calculates a straight line between two waypoints.   More complex controllers are necessary if the goal state is of higher dimension. Non-holomonic approximations would enter into this category.   Finally, we can consider constraints along the trajectory, such as staying a certain distance away from known obstacles.

The planned path can be refined as the process continues, so for instance the autonomous vehicle could start its trajectory after only several seconds of calculation, knowing that the calculation is sufficient for the immediate future.

The Flight Director (currently unwritten) can usurp the path planner at any time. This would be logical in the case of the Flight Director detecting an imminent collision, or perhaps just the Flight Director considering that a condition has been fulfilled.

2) Path manager: The path manager receives an ordered list of switching loci (see below). It's function in the navigation hierarchy is to pass the appropriate path segment to the path follower, and monitor the path follower's progress.

![image](https://f.cloud.github.com/assets/1118185/435010/c5a2baea-af8b-11e2-8585-b0340d5e8e16.png)

3) Path follower: The path follower's job is narrowly limited to determining what control inputs are necessary to keep the vehicle on the path segment indicated by the path manager. (The path follower can also inform the path manager of its state errors, but that is not currently implemented here.)

*Switching loci*: There is a new notion introduced here. Switching loci are the intermediate points that guide a vehicle between user-defined waypoints.

![image](https://f.cloud.github.com/assets/1118185/435003/cb1ee9d6-af8a-11e2-9998-e322b898ec8e.png)

The path planner currently supports two algorithms: direct and direct-with-filetting. Direct is appropriate for holonomic vehicles, whereas the filleting is appropriate for dubins carts, i.e. non-holonomic vehicle classes.

Direct with filleting: This fills in angles with arcs, so that the vehicle can make a smooth transition from one path segment to the next.

Path when angle is sufficiently obtuse. The path is the green band.
![Screen Shot 2013-04-28 at 12 22 41 AM](https://f.cloud.github.com/assets/1118185/434994/0372fefa-af89-11e2-86d9-1a744f088098.png)

Path when angle is sufficiently acute that the fillet would be pushed further away than one radius worth. The path is the green band.
![Screen Shot 2013-04-28 at 12 22 15 AM](https://f.cloud.github.com/assets/1118185/434995/0bbd85bc-af89-11e2-95b4-410c74714f6f.png)

The path planner and path manager have been heavily tested in simulation with X-Plane, and the path follower heavily tested in autonomous flight. I think I have a very solid idea of the planner and manager's limitations, only one of which requires further attention (handling the overshoot case). Here are some screenshots of the path planner/manager/follower in action:

Old logo. Good test because many of the points are very close together. Also tested with moderate turbulence and a 10kts windspind, equalling 40% of the plane's cruise speed along the path.
![OP logo 10kts of wind](https://f.cloud.github.com/assets/1118185/434986/7e051a8c-af88-11e2-9dbb-cdfa2c49c6ea.png)

Random test with lots of acute angles, showing how it works. In practice, the path follower is a little slow so it doesn't track as nicely as I'd like, but that's a different problem.
![Path planning with half-planes and filleting](https://f.cloud.github.com/assets/1118185/434987/7e1ac724-af88-11e2-81c7-aa24c69872d9.png)

Position hold
![Screen Shot 2013-04-26 at 1 19 43 PM](https://f.cloud.github.com/assets/1118185/434988/7e30b8fe-af88-11e2-8fcb-2214fdc2d89d.png)

Return to home
![Screen Shot 2013-04-26 at 1 21 49 PM](https://f.cloud.github.com/assets/1118185/434989/81a4dfb0-af88-11e2-8e81-4ccfbf055b3f.png)

New path. This is a really good one for testing on dubins carts, since the corners are all very sharp (144 degrees). Maybe I should think about how wide the circle is relative to the stars points.
![Screen Shot 2013-04-26 at 11 11 35 PM](https://f.cloud.github.com/assets/1118185/434990/8b56820c-af88-11e2-9aac-2a9e13aaa5cc.png)