Guidance vector field

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. The algorithm is based on the idea of following a guiding vector field which is constructed from the implicit function that describes the desired (possibly time-varying) trajectory.

For fixed-wings the output of the algorithm can be directly expressed in terms of the bank angle of the UAV in order to achieve coordinated turns. Furthermore, the algorithm can be tuned offline such that physical constraints of the UAV, e.g. the maximum bank angle, will not be violated in a neighborhood of the desired trajectory.

This work is based on the paper Guidance algorithm for smooth trajectory tracking of a fixed wing UAV flying in wind flows, in the proceedings of ICRA 2017.

gvf_gif
Tracking an ellipitical and sinusoidal trajectories by a fixed-wing in Paparazzi.

How does it work?

We start from the implicit equation of the trajectory, for example for a circumference we have that \varphi(x,y) = x^2 + y^2 - R^2, where x, y and R are the x, y coordinates with respect to HOME (O in the figure below) and R is the radius of the circumference. Note that when the vehicle is over the desired trajectory, then \varphi(x,y) = 0, otherwise it will be different from zero. We will use this concept of “level set” to define the notion of distance to the trajectory, namely e := \varphi(x,y). It has to be noted that this is different from the Euclidean distance (see figure below).

For the sake of clarity we will consider just 2D trajectories and parallel to the ground. Let us stack the x and y coordinates in p := \begin{bmatrix}x & y\end{bmatrix}^T and consider the following kinematical model for our vehicle

\begin{cases} \dot p &= sm(\psi) + w \\ \dot\psi &= u, \end{cases}

where s\in\mathbb{R}^+ is a constant that can be considered as the airspeed, m = \begin{bmatrix}\cos(\psi) & \sin(\psi)\end{bmatrix}^T with \psi\in(-\pi, \pi] being the attitude yaw angle, w\in\mathbb{R}^2 is a constant with respect to O representing the wind and u is the control action that will make the vehicle to turn. We also notice that the course heading \chi\in(-\pi, \pi], i.e. the direction the velocity vector \dot p is pointing at, in general is different from the yaw angle \psi because of the wind.

gvf

Given a desired trajectory, we compute its normal

n(p) := \nabla \varphi(p)

and its tangent

\tau(p) = En(p), \quad E=\begin{bmatrix}0 & 1 \\ -1 & 0\end{bmatrix}

The idea is to steer the vehicle such that it follows the direction given by the unit vector calculated from

 \dot p_d(p) := \tau(p) - k_e e(p)n(p),

where k_e is a positive gain. At each point p we can build a unit vector from \dot p_d(p). This collection of vectors is our Guidance Vector Field. Note that when the error is zero, then we are just tracking the tangent to the trajectory.

The algorithm needs for work the measurements of \dot p (typically derived from GPS), position p w.r.t. HOME (typically derived from GPS too) and yaw angle \psi or sideslip angle \beta = (\psi - \chi). The algorithm still works quite well in practice with only \dot p and p, in other words, you will only need to have installed a GPS.

The algorithm is already available and ready to use in the opensource project Paparazzi, the free autopilot.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s