### Guidance algorithm for tracking smooth trajectories

I have written with my friend Yuri Kapitanyuk (the main idea was from him) an algorithm for solving the problem of tracking smooth curves by an unmanned aerial vehicle travelling with a constant airspeed and under a wind disturbance. Basically, if the trajectories are twice differentiable, i.e., they are continuous, without "spikes", etc. then the algorithm is capable to construct an attractive vector field around the trajectory in order to guide the vehicle.

The algorithm has been successfully tested on different fixed wings and it is available in Paparazzi. It has been written as an independent module there, so it should be easy to port to other platforms if somebody is interested. In fact, the code of the algorithm has been split in two parts there.

• The first one is the core of the algorithm and it is transparent for the user that wants to include its own custom trajectories.
• The second one corresponds to the trajectories, where the user has to provide the equations of the trajectory, its gradient and its Hessian (the gradient of the gradient).

That makes the algorithm very modular. As an examples (in the gif) I have already implemented in Paparrazi ellipses (they cover the case of circumferences) and sinusoidals (they also cover the particular case of straight lines a.k.a. zero frequency).

If one is interested in the details and how the algorithm works, I have written a more detailed post in my blog http://dobratech.com .

Cheers,

• @Huges

What is the objective of having a smooth trajectory ?

The same one as for tracking straight lines or circles. Now you have a broader class of trajectories.

in winds ?

Wind never helps you to track a path, it is a disturbance.

Is it a pure math exercise or does this have a utility in practice ?

Same answer as in the first question. What is the utility of following straight lines or circumferences?

• MR60

What is the objective of having a smooth trajectory ? in winds ? Is it a pure math exercise or does this have a utility in practice ?

• Very nice. Will have to add this as a feature request for Arduplane.

I had a quick look at your other work. Awesome stuff! I like! :-)

Keep it up. I think there's a whole bunch of things that can be done to optimise UAV flight strategies still. With better monitoring and control comes better hardware solutions as well (ie we can start leaving stuff out of the plane and still end up with a more capable platform) In particular the swarming behaviour is of interest to me, especially when decision making becomes a shared and distributed process. My mantra is decentralise, distribute and diversify. So "swarming intelligence" is key to those goals. ;-)

• @JB

This is another of the ``beauties'' of the algorithm. The code of the algorithm is the function gvf_control_2D (line 97) at https://github.com/paparazzi/paparazzi/blob/master/sw/airborne/modu...

as you can see there, it is rather inexpensive. In fact in the paper, it is just to read your position and velocity GPS and evaluate the expression in Theorem 3.4. Such a expression is just a couple of trigonometric operations, additions and multiplications. In our Paparazzi setup (typical STM32) I guess it is executed at 300Hz?

In the experiment from the paper we were running it at 60Hz.

paparazzi/paparazzi
Paparazzi is a free and open-source hardware and software project for unmanned (air) vehicles. This is the main software repository. - paparazzi/papa…
• Hector thanks for the additional info. Looks and sounds great. Might have to see if I can motivate some devs to adopt it in Arduplane! I think it would be a great feature that can potentially give some "pre-emptive" rather than just "reactive" control to automated flight. I'm assuming all of this is being calculated on the FC MCU whilst in flight? Does it have much processor overhead?

• @Martin

Actually, the algorithm does not require constant wind, just to be "bounded", i.e. to have the ground speed far from zero. The algorithm needs the yaw and the heading course in order to calculate the cosine of the sideslip. This is an "inner" calculation, in fact, it is not needed for the construction of the vector field but for steering the plane to the vector field.

In practice, we have found that by substituting the cosine of beta (sideslip) by one (even for actual big cross wind), and in the equations by changing all the "yaws" by "course headings" the algorithm stills works quite well. Therefore simplifying the sensing requirements (only GPS measurements). This is the current version implemented in Paparazzi.

While it is not strictly the same analytic solution provided in the paper, after several field test with different airplanes (from 500gr up to 2Kg and with wind speeds beeing almost the 50% of the nominal airspeed), we are quite happy with this (engineering) approximation where the sideslip is not longer required.

• @Hector, how has the wind be provided to the algorithm? Is it assumed to be constant on all altitudes? How can wind changes be quickly considered?

• Hi JB, thanks!

Indeed, the algorithm accepts the popular splines as trajectories. Therefore, typical optimization routines for setting lateral accelerations offline can be applied.

Another nice thing about the algorithm is that one can tune "how aggressive" is the vector field with a single gain. The required maximum bank angle can be computed/assigned off-line, so planes with limited maneuvering can still make it.

We can combine both things above. For example, you give a set of waypoints (or better "gates" like in downhill sky), the constraints of the airplane such as maximum/minimum expected ground speed and maximum bank angle. Then it should be possible to generate a set of splines to be followed. In a future, (not very close unfortunately xD), I would have students to work in the integration of such a stuff.

Does it work in 3D?

Yes, it totally works in 3D, so you can define your desired climbing rate. In fact, it works with also time varying trajectories. For example, consider a 3D paraboloid whose 3D equation is X(x,y,z) = 0. Now consider a tilted plane (not parallel to the ground) Y(x,y,z) = 0. The following new equation X - Y = 0 defines their intersection! The following is a bit extreme for an airplane, but you will get the idea:

Now the paraboloid can be fixed, but you can move over time the plane (for example up and down in order to cover the perimeter of the paraboloid in a spiral fashion), now you will have Y(x,y,z,t). Do not let the maths to scare you, as an user, you only have to provide (once) to the algorithm the explicit X, Y and their gradients and Hessians.

In my opinion, this approach has a lot of potential to be exploited! Since the math for the user that wants to define his custom trajectories are very simple. I would refer to the Paparazzi entry for more details about how to actually define a trajectory in your code. Of course, for the end-user all of this is transparent, the guy only needs to choose in his flightplan a preprogammed trajectory done by the developers.

• Nice work Hector, and thanks for the share. I Like the more analog type flight paths created with your algorithm. This will likely improve flight efficiency and also allow mission planning to be more robust without hitting the max control outputs for steering (like max roll). Does this algorithm also work for height changes as well, so in "3D"?

• Moderator

@Hector

Many thanks, waiting for next release :)))

This reply was deleted.