Neat Navigation, Paths and Waypoints.

[Crubier]

Ok, apparently there is a lot of work undergoing here about improving, simplifying, refactoring navigation etc...

I like James idea of simplifying things to the maximum in a first place.

From there we will be able to improve things, and make a leap forward.

And before going into big overhaul, I think we should discuss about the bigger picture.
We should take some distance, and talk about our "vision" about the future of navigation.

This topic is here for that purpose. 
Feel free to discuss and give new ideas.

[Crubier]

I have been thinking about this topic for a long time, and I came to a conclusion.

I am going to advocate for the use of Bezier curves instead of lists of waypoints, path actions, parameters etc...

Here are my arguments :

• Physical consistency
• Now : Paths are represented as lists of waypoints, which form a C0-continuous curve
• Problem : The trajectory of a physical object in space is a C1-continuous curve
• Solution : Use Cubic Bezier curves, which form a C1-continuous curve if they are assembled correctly
• Benefit : The representation of paths would fit perfectly with the actual physics, allowing to validate paths for a certain type of vehicle, etc etc...
• Code simplification
• Now : In order to fulfill certain needs, path can be made of different type of objects : straight segments, arcs. This is managed with different path actions, or modes.
• Problem : These different type of elements add complexity to the flight code, gcs code. Transition between different modes makes things more complex, and maybe less safe
• Solution : Use Bezier cuves only to represent the different shapes and much more !
• Benefit : No mode transitions, only one kind of object, flight code would be much simpler and easy to validate.
• Memory efficiency
• Now : Path are represented as segments, and there are mode parameters (circle radius etc...) for other modes.
• Problem : Any complex flight path needs a lot of segments in order to be smooth. mode parameters take space which is often unused.
• Solution : Bezier curves
• Benefit : Bezier curves are much more expressive than segments. This allows to save a lot of waypoints. Same path -> less waypoints -> less memory used. No need for mode parameters anymore, at least for trajectory management.
• Evolution possibilities
• Now : There are basically two hard coded shapes : segment and circle arcs.
• Problem : It is impossible or very hard to add any complex flight path. (Ellipse, Spiral, 8 figure ...)
• Solution : GCS "Macros" to generate different shapes. Flight code only has to process bezier curves, does not care about higher level abstractions. 
• Benefit : Any 3D trajectory can be created super easily, there is absolutely no limit !
• Third party compatibility
• Now : Flight paths are represented in a quite unique maneer. 
• Problem : There is almost no possibility to import/export 3D flight paths, automated path generation is hard...
• Solution : Bezier curves
• Benefit : Bezier curves have a huge support everywhere, there are tons of libraries and algorithms around them. Automatic flight path genreration and reconfiguration is feasible.
• Usability
• Now : Flight paths are specified in the GCS, need a lot of parameters, (circle radius etc...)
• Problem : Not as easy as drawing a shape in Inkscape or any drawing program !
• Solution : Bezier curves
  
• Benefit : Thanks to the unification into bezier curves, it would be really easy to draw paths in the GCS, using points and handles, as in any drawing software !
• Flight path quality
• Now : Paths are made of segments and arcs
• Problem : Possibilities are quite limited, especially for aerial video. Segment based paths are not C1-continuous and can lead to oscillations of the vehicle on turns
• Solution : You name it !
• Benefit : Continuous, smooths flight paths

I think I have a lot more to say about this. But we can start discussing about it here.

[Kenn Sebesta]

I think this is great to be thinking along these lines. peabody124 and I had discussed using Frenet-Serret to parameterize the curves, but J. Baillieul feels that roll-pitch-velocity is a more natural expression. 

You present a lot of good advantages of Bezier curves, which themselves could be parameterized into Frenet-Serret or RPV space. I would like to point out that you are absolutely correct that the curve through space is particularly poorly approximated by a piecewise continuous function. However, we should not forget that the UAV is in SE(3), and so we need to find expressions for roll, pitch, and yaw which are also continuously differentiable, alongside the T(3) NED representation. Indeed, GRASP solves for their aggressive flight in C4 (although their controller is only C2), so we have good lower bounds on how smooth our trajectories through SE(3) need to be.

Unfortunately, I can't comment more than for the next couples weeks, but I will give your post a more thoughtful response when I get back home.

[Crubier]

You are totally right Kenn, my explanation about bezier curves only deals with the vehicle position, not orientation.

Basically one way to represent the motion of a vehicle in space is to represent :

• Its position in T(3) NED coordinates
• Its orientation in SO(3) Rotation from NED axis
  

I would like to advocate that these different concerns should be kept separated as much as possible.

Indeed, in the general case, the orientation of a body in space is not related to its orientation. 

As an example, I have a quadrotor frame with tiltable arms, allowing it to translate without changing its orientation. It has 6 degrees of freedom (3Rotations + 3Translations).

I really believe that this kind of vehicles on which speed and orientation are not (or less) correlated will continue to develop, and that is why we need to have an approach which does not correlate attitude with position.

I feel that when one says that roll-pitch-velocity is a more natural expression, one thinks about a particular kind of vehicle (fixed wing, heli...).

But in the general case, we need to be able to specify speed and attitude separately.

Depending on the capabilities of vehicles, there should be a priority, defining if the vehicle should try to maintain the commanded attitude, or the commanded trajectory, if it cannot do both.

If I had to represent attitude command right now, I think that I would use bezier curves in the space of normalized quaternions. This would allow to have smooth C2 rotations and easy interpolation during transitions between 2 commanded attitudes, while being very computing power-effective.

This would also avoid the infamous problem of gimbal lock which we have with (Pitch Yaw Roll). 

But that is just a quick opinion, I did not think a lot about this yet.

[peabody124]

Sorry for the following rambling post:

Just to link to the previous discussions on this:

https://groups.google.com/forum/#!topic/phoenixpilot/kTmHSAh1Bic

https://groups.google.com/forum/#!topic/phoenixpilot/uuaLqP_NLeQ

and also some relevant discussion/arguing :) here https://github.com/PhoenixPilot/PhoenixPilot/pull/40

pending review https://github.com/PhoenixPilot/PhoenixPilot/pull/68

One thing that I like about the current code (quite a lot) is that the representation is relatively opaque to most of the code.  That was by design.  With #68 you'll notice that the only place in the pathplanner where logic about the detail of the path is used is in the libraries (e.g. flight/Libraries/path_calculation/path_calculation_simple.c) . The PathDesired object is passed into flight/Libraries/paths.c so the pathfollower logic never even has to know what type of path segment it is following.

So the good news is basically to try these things out (not counting the work to make the GCS represent them) is just a matter of changing those two files and the pathdesired uavo.  I'm not proficient enough in these things to make a good judgement of the right representation.  I would like to not commit too early to one without really trying it out and seeing what it buys us beyond a simpler representation with lots of closely placed waypoints.

A point Kenn made to me a while ago which was something I hadn't considered (so I bring it up for others that might not have) is that in some cases the calculation of the desired path might be a very hard problem that takes a while to solve.  Then actually following that path is done as well as possible.  If you deviate from it, then you don't bother recomputing a new optimal path because the first step of calculating it somewhat accounted for reasonable deviations and found something that was robust enough.  For example at OP Corvus pointed out a situation where the fixed wing path follower flies backwards.  Kenn pointed out this ultimately happens because an invalid path was requested that wasn't realizable so it shouldn't have ever reached that state.  However, if it ever does it would be nice to make sure the follower is as robust as it can be made.

The GCS PathPlanner plugin is great because it clearly isolates the logic for some of this into an MVC architecture.  We can have one representation of the flight plan for visualization purposes and internally in the GCS, but when we send it to the UAV it might be at this point we run calculations and consider smoothness in certain spaces (and differently for fixed wing / multirotor accounting for all sorts of physics) but at the end what we want to follow might be perfectly represented by mostly curves and lines (which are supported in the firmware currently).

Finally we are inching towards a three tier implementation:

1. FSM - high level logic for transitions between path segments (waypoints currently).  Right now what Kenn would like it to support is "everything".  While we are playing with the lower level parts though I'd like to keep this simple in the main code.

2. "path planning" - ironically now not done in the "PathPlanner" module.  this is the calculation when we select a new waypoint of the representation to get this.  Currently there is just the simple implementation that basically copies the waypoint information into the path desired.  It might even be nice (although it would blow a few bytes of ram) for the waypoint to be identical to the path desired so it could be blindly copied over.  However, this probably isn't worth it since this wouldn't be compatible with the FSM making arbitrary transitions.

3. path following - this is split into two sub components.  the library component that takes the current position (and should include velocity) and calculates the desired trajectory, and the vehicle specific part while deals with safely attempting to achieve that trajectory.

Finally, I'm sure some things might be doable better/cleaner by collapsing tiers of this architecture into big monolithic blocks of code (e.g. more tightly integrating the planning with the following).  I would suggest anything like this be written as an entirely independent module rather than be tried to shoe horn into the existing architecture.  Let's keep the main code as clean as we can for as long as possible so these types of experiments are easy :)