Trajectory Tracking for Aerial Robots: an Optimization-Based Planning and Control Approach

Abstract

In this work, we present an optimization-based trajectory tracking solution for multirotor aerial robots given a geometrically feasible path. A trajectory planner generates a minimum-time kinematically and dynamically feasible trajectory that includes not only standard restrictions such as continuity and limits on the trajectory, constraints in the waypoints, and maximum distance between the planned trajectory and the given path, but also restrictions in the actuators of the aerial robot based on its dynamic model, guaranteeing that the planned trajectory is achievable. Our novel compact multi-phase trajectory definition, as a set of two different kinds of polynomials, provides a higher semantic encoding of the trajectory, which allows calculating an optimal solution but following a predefined simple profile. A Model Predictive Controller ensures that the planned trajectory is tracked by the aerial robot with the smallest deviation. Its novel formulation takes as inputs all the magnitudes of the planned trajectory (i.e. position and heading, velocity, and acceleration) to generate the control commands, demonstrating through in-lab real flights an improvement of the tracking performance when compared with a controller that only uses the planned position and heading. To support our optimization-based solution, we discuss the most commonly used representations of orientations, as well as both the difference as well as the scalar error between two rotations, in both tridimensional and bidimensional spaces SO(3) and SO(2). We demonstrate that quaternions and error-quaternions have some advantages when compared to other formulations.

Appendices

Appendix A: Appendix to Trajectory Definition

To ease the formulation of the trajectory planner presented in Section 7, the definition of the trajectory can be expressed using the compact formulation presented along with this Appendix A.

1.1 A.1 Position and Derivatives of a Polynomial

The position of a polynomial segment defined in Eq. 67, can be written using the following compact nomenclature:

$$ \begin{array}{@{}rcl@{}} p_{i,j}(\tau_{i}) &=& \boldsymbol{l}_{\boldsymbol{x},m_{i},0}(\tau_{i}) \cdot \boldsymbol{b}_{i,j,:} \\ &=& \left[ 1 \quad \tau_{i} \quad \tau_{i}^2 \quad ... \quad \tau_{i}^{m_{i}} \right] \cdot \left[ \begin{array}{c} b_{i,j,0} \\ b_{i,j,1} \\ b_{i,j,2} \\ ... \\ b_{i,j,m_{i}} \end{array} \right] \end{array} $$

(89)

$$ \begin{array}{@{}rcl@{}} p_{i,j}(\tau_{i}) &=& {(\boldsymbol{b}_{i,j,:})}^{T} \cdot \boldsymbol{r}_{\boldsymbol{x},m_{i},0}(\tau_{i}) \\ &=& \left[ b_{i,j,0} \quad b_{i,j,1} \quad b_{i,j,2} \quad ... \quad b_{i,j,m_{i}} \right] \cdot \left[ \begin{array}{c} 1 \\ \tau_{i} \\ \tau_{i}^2 \\ ... \\ \tau_{i}^{m_{i}} \end{array} \right]\\ \end{array} $$

(90)

The l_m-th time-derivative of the position defined in Eq. 68, can be written as:

$$ \begin{array}{@{}rcl@{}} p_{i,j}^{(l_{m})}(\tau_{i}) = \frac{{\mathrm{d}}^{l_{m}} p_{i,j}}{{\mathrm{d}} \tau_{i}^{l_{m}}} &=& \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}}(\tau_{i}) \cdot \boldsymbol{b}_{i,j,:} \end{array} $$

(91)

$$ \begin{array}{@{}rcl@{}} &=& {(\boldsymbol{b}_{i,j,:})}^{T} \cdot \boldsymbol{r}_{\boldsymbol{x},m_{i},l_{m}}(\tau_{i}) \end{array} $$

(92)

where the k-th element of the matrices:

$$ \begin{array}{@{}rcl@{}} {\boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}}(\tau_{i})}_{1,k}&=&{\boldsymbol{r}_{\boldsymbol{x},m_{i},l_{m}}(\tau_{i})}_{k,1} \\ &=&\left( \prod\limits_{l=k-(l_{m}-1)}^{k} l \right) \cdot \tau_{i}^{k-l_{m}} \end{array} $$

(93)

The following equivalence can be easily extracted:

$$ {\boldsymbol{r}_{\boldsymbol{x},m_{i},l_{m}}(\tau_{i})} = \left( {\boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}}(\tau_{i})}\right)^{T} $$

(94)

Additionally, it can be easily demonstrated that:

$$ \begin{array}{@{}rcl@{}} \frac{{\mathrm{d}} \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}}(\tau_{i})}{{\mathrm{d}} \tau_{i}} &=& \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m+1}}(\tau_{i}) \end{array} $$

(95)

$$ \begin{array}{@{}rcl@{}} \frac{{\mathrm{d}} \boldsymbol{r}_{\boldsymbol{x},m_{i},l_{m}}(\tau_{i})}{{\mathrm{d}} \tau_{i}} &=& \boldsymbol{r}_{\boldsymbol{x},m_{i},l_{m+1}}(\tau_{i}) \end{array} $$

(96)

The norm of the magnitude, ${p}_{i,j}^{(l_{m})}(\tau _{i})$, can be calculated as:

$$ \begin{array}{@{}rcl@{}} {\| {p}_{i,j}^{(l_{m})}(\tau_{i}) \|}_{2}^{2} &=&\left( {p}_{i,j}^{(l_{m})}(\tau_{i}) \right)^{T} \cdot {p}_{i,j}^{(l_{m})}(\tau_{i})\\ &=&\left( { \boldsymbol{b}_{i,j,:} }\right)^{T} \cdot { \boldsymbol{r}_{\boldsymbol{x},m_{i},l_{m}} } (\tau_i) \cdot { \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}} } (\tau_i) \cdot { \boldsymbol{b}_{i,j,:} }\\ &=&\left( { \boldsymbol{b}_{i,j,:} }\right)^{T} \cdot { \boldsymbol{rl}_{\boldsymbol{x},m_{i},l_{m}} } (\tau_i) \cdot { \boldsymbol{b}_{i,j,:} } \end{array} $$

(97)

1.2 A.2 State of a Polynomial

The state $\boldsymbol {x}_{i,j,l_{0}:l_{m}}$ of a polynomial can be defined as:

$$ \boldsymbol{x}_{i,j,l_{0}:l_{m}}(\tau_{i}) = \left[ p_{i,j}^{(l_{0})} \quad p_{i,j}^{(l_{1})} \quad ... \quad p_{i,j}^{(l_{m})} \right]^{T} $$

(98)

Which is calculated using the following compact nomenclature:

$$ \begin{array}{@{}rcl@{}} \boldsymbol{x}_{i,j,l_{0}:l_{m}}(\tau_{i}) &=& \boldsymbol{L}_{\boldsymbol{x},m_{i},l_{0}:l_{m}} (\tau_{i}) \cdot \boldsymbol{b}_{i,j,:} \end{array} $$

(99)

$$ \begin{array}{@{}rcl@{}} \left( \boldsymbol{x}_{i,j,l_{0}:l_{m}}(\tau_{i})\right)^{T} &=& {(\boldsymbol{b}_{i,j,:})}^{T} \cdot \boldsymbol{R}_{\boldsymbol{x},m_{i},l_{0}:l_{m}}(\tau_{i}) \end{array} $$

(100)

where

$$ \begin{array}{@{}rcl@{}} \boldsymbol{L}_{\boldsymbol{x},m_{i},l_{0}:l_{m}} (\tau_{i})= \left[ \begin{array}{c} \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{0}}(\tau_{i})\\ \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{1}}(\tau_{i})\\ ...\\ \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}}(\tau_{i}) \end{array} \right] \end{array} $$

(101)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{R}_{\boldsymbol{x},m_{i},l_{0}:l_{m}}(\tau_{i}) = (\boldsymbol{L}_{\boldsymbol{x},m_{i},l_{0}:l_{m}}(\tau_{i}))^{T} \end{array} $$

(102)

In the same way than in Eqs. 95 and 96, it can be easily demonstrated that:

$$ \begin{array}{@{}rcl@{}} \frac{{\mathrm{d}} \boldsymbol{L}_{\boldsymbol{x},m_{i},l_{0}:l_{m}}(\tau_{i})}{{\mathrm{d}} \tau_{i}} = \boldsymbol{L}_{\boldsymbol{x},m_{i},l_{1}:l_{m+1}}(\tau_{i}) \end{array} $$

(103)

$$ \begin{array}{@{}rcl@{}} \frac{{\mathrm{d}} \boldsymbol{R}_{\boldsymbol{x},m_{i},l_{0}:l_{m}}(\tau_{i})}{{\mathrm{d}} \tau_{i}} = \boldsymbol{R}_{\boldsymbol{x},m_{i},l_{1}:l_{m+1}}(\tau_{i}) \end{array} $$

(104)

1.3 A.3 Coefficients of a polynomial

Given the state of a polynomial, $\boldsymbol {x}_{i,j,l_{0}:l_{m}}$, at a certain time τ_i, the coefficients of the polynomial, b_i,j,:, can be calculated using Eq. 100, as:

$$ \boldsymbol{b}_{i,j,:} = \left( \boldsymbol{L}_{\boldsymbol{x},m_{i},l_{0}:l_{m}}(\tau_{i}) \right)^{-1} \cdot \boldsymbol{x}_{i,j,l_{0}:l_{m}}(\tau_{i}) $$

(105)

with the condition that l₀ = 0 and l_m = m_i.

The coefficients of a polynomial, b_i,j,:, can be calculated from its initial state, $\boldsymbol {x}_{i,j,0:m_{i}}(0)$, using (105).

$$ \boldsymbol{b}_{i,j,:} = \left( \boldsymbol{L}_{\boldsymbol{x},m_{i},0:m_{i}}(0) \right)^{-1} \cdot \boldsymbol{x}_{i,j,0:m_{i}}(0) $$

(106)

being therefore the value of the coefficients, independent of the time, Δτ_i.

1.4 A.4 Initial and Final States of a Polynomial

The initial state of a polynomial, $\boldsymbol {x}_{i,j,0:m_{i}}(0)$, can be calculated following (100), as:

$$ \boldsymbol{x}_{i,j,0:m_{i}}(0) = \boldsymbol{L}_{\boldsymbol{x},m_{i},0:m_{i}}(0) \cdot \boldsymbol{b}_{i,j,:} $$

(107)

The final state of a polynomial, $\boldsymbol {x}_{i,j,0:m_{i}}({\varDelta } \tau _{i})$, can be calculated from its initial state as follows:

$$ \begin{array}{@{}rcl@{}} \boldsymbol{x}_{i,j,0:m_{i}}({\varDelta} \tau_{i}) &=& \boldsymbol{L}_{\boldsymbol{x},m_{i},0:m_{i}}({\varDelta} \tau_{i}) \cdot \boldsymbol{b}_{i,j,:} \\ &=& \boldsymbol{L}_{\boldsymbol{x},m_{i},0:m_{i}}({\varDelta} \tau_{i}) \cdot \left( \boldsymbol{L}_{\boldsymbol{x},m_{i},0:m_{i}}(0) \right)^{-1} \cdot \boldsymbol{x}_{i,j,0:m_{i}}(0)\\ \end{array} $$

(108)

1.5 A.5 Linear Variables of a Segment

The full-dimensional position of the segment is defined as:

$$ \boldsymbol{p}_{i,:}\left( \tau_i \right) ={\left[{p}_{i,x}\left( \tau_i \right) \quad {p}_{i,y}\left( \tau_i \right) \quad {p}_{i,z}\left( \tau_i \right)\right]}^{T} $$

(109)

Which is calculated using the following compact nomenclature:

$$ \begin{array}{@{}rcl@{}} \boldsymbol{p}_{i,:}\left( \tau_i \right)&=&{\boldsymbol{l}_{\boldsymbol{x},m_{i},0}} (\tau_i) \cdot {\boldsymbol{b}_{i,xyz,:}} \end{array} $$

(110)

$$ \begin{array}{@{}rcl@{}} \left( \boldsymbol{p}_{i,:}\left( \tau_i \right) \right)^{T}&=&\left( \underline{\boldsymbol{b}_{i,xyz,:}}\right)^{T} \cdot \underline{\boldsymbol{r}_{\boldsymbol{x},m_{i},0}} (\tau_i) \end{array} $$

(111)

where

$$ \begin{array}{@{}rcl@{}} \underline{ \boldsymbol{b}_{i,xyz,:} } &=& \left[ \begin{array}{c} \boldsymbol{b}_{i,x,:} \\ \boldsymbol{b}_{i,y,:} \\ \boldsymbol{b}_{i,z,:} \end{array} \right] \end{array} $$

(112)

$$ \begin{array}{@{}rcl@{}} \underline{ \boldsymbol{l}_{\boldsymbol{x},m_{i},0} } (\tau_i) &=& \left[ \begin{array}{ccc} \boldsymbol{l}_{\boldsymbol{x},m_{i},0} (\tau_{i}) & \boldsymbol{0}_{1 \times (m_{i}+1)} & \boldsymbol{0}_{1 \times (m_{i}+1)} \\ \boldsymbol{0}_{1 \times (m_{i}+1)} & \boldsymbol{l}_{\boldsymbol{x},m_{i},0} (\tau_{i}) & \boldsymbol{0}_{1 \times (m_{i}+1)} \\ \boldsymbol{0}_{1 \times (m_{i}+1)} & \boldsymbol{0}_{1 \times (m_{i}+1)} & \boldsymbol{l}_{\boldsymbol{x},m_{i},0} (\tau_{i}) \end{array} \right] \end{array} $$

(113)

$$ \begin{array}{@{}rcl@{}} \underline{ \boldsymbol{r}_{\boldsymbol{x},m_{i},0} } (\tau_i) &=& \left( \underline{ \boldsymbol{l}_{\boldsymbol{x},m_{i},0} } (\tau_i) \right)^{T} \end{array} $$

(114)

The l_m-th time-derivative of the position can be expressed as:

$$ \boldsymbol{p}_{i,:}^{(l_{m})}\left( \tau_i \right) =\frac{{{\mathrm{d}}}^{l_{m}} \boldsymbol{p}_{i,:}}{{{\mathrm{d}}} \tau_{i}^{l_{m}}} =\left[{p}_{i,x}^{(l_{m})}\left( \tau_i \right) \quad {p}_{i,y}^{(l_{m})}\left( \tau_i \right) \quad {p}_{i,z}^{(l_{m})}\left( \tau_i \right) \right]^{T} $$

(115)

Which is calculated using the following compact nomenclature:

$$ \begin{array}{@{}rcl@{}} \boldsymbol{p}_{i,:}^{(l_{m})}\left( \tau_i \right) &=& \underline{ \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}} } (\tau_i) \cdot \underline{ \boldsymbol{b}_{i,xyz,:} } \end{array} $$

(116)

$$ \begin{array}{@{}rcl@{}} \left( \boldsymbol{p}_{i,:}^{(l_{m})}\left( \tau_i \right) \right)^{T}&=&\left( \underline{ \boldsymbol{b}_{i,xyz,:} }\right)^{T} \cdot \underline{ \boldsymbol{r}_{\boldsymbol{x},m_{i},l_{m}} } (\tau_i) \end{array} $$

(117)

where

$$ \begin{array}{@{}rcl@{}} \underline{ \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}} } (\tau_i) &=& \left[ \begin{array}{ccc} \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}} (\tau_{i}) & \boldsymbol{0}_{1 \times (m_{i}+1)} & \boldsymbol{0}_{1 \times (m_{i}+1)} \\ \boldsymbol{0}_{1 \times (m_{i}+1)} & \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}} (\tau_{i}) & \boldsymbol{0}_{1 \times (m_{i}+1)} \\ \boldsymbol{0}_{1 \times (m_{i}+1)} & \boldsymbol{0}_{1 \times (m_{i}+1)} & \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}} (\tau_{i}) \end{array} \right] \end{array} $$

(118)

$$ \begin{array}{@{}rcl@{}} \underline{ \boldsymbol{r}_{\boldsymbol{x},m_{i},l_{m}} } (\tau_i) &=& \left( \underline{ \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}} } (\tau_i) \right)^{T} \end{array} $$

(119)

It can be easily demonstrated that:

$$ \begin{array}{@{}rcl@{}} \frac{{\mathrm{d}} \underline{ \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}} } (\tau_i)}{{\mathrm{d}} \tau_i} &=& \underline{ \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m+1}} } (\tau_i) \end{array} $$

(120)

$$ \begin{array}{@{}rcl@{}} \frac{{\mathrm{d}} \underline{ \boldsymbol{r}_{\boldsymbol{x},m_{i},l_{m}} } (\tau_i)}{{\mathrm{d}} \tau_i} &=& \underline{ \boldsymbol{r}_{\boldsymbol{x},m_{i},l_{m+1}} } (\tau_i) \end{array} $$

(121)

The norm of the magnitude, $\boldsymbol {p}_{i,:}^{(l_{m})}(\tau _{i})$, can be calculated as:

$$ \begin{array}{@{}rcl@{}} {\| \boldsymbol{p}_{i,:}^{(l_{m})}(\tau_{i}) \|}_{2}^{2} &=& \left( \boldsymbol{p}_{i,:}^{(l_{m})}(\tau_{i}) \right)^{T} \cdot \boldsymbol{p}_{i,:}^{(l_{m})}(\tau_{i}) \\ &=& \left( \underline{ \boldsymbol{b}_{i,xyz,:} }\right)^{T} \cdot \underline{ \boldsymbol{r}_{\boldsymbol{x},m_{i},l_{m}} } (\tau_i) \cdot \underline{ \boldsymbol{l}_{\boldsymbol{x},m_{i},l_{m}} } (\tau_i) \cdot \underline{ \boldsymbol{b}_{i,xyz,:} } \\ &=& \left( \underline{ \boldsymbol{b}_{i,xyz,:} }\right)^{T} \cdot \underline{ \boldsymbol{rl}_{\boldsymbol{x},m_{i},l_{m}} } (\tau_i) \cdot \underline{ \boldsymbol{b}_{i,xyz,:} } \end{array} $$

(122)

Appendix B: Appendix to Evaluation and Results

This Appendix B gathers extra information and the raw data of the experiments presented in Section 9.

Figures 29 and 30 graphically represent the total time of the trajectory tracking computed by the trajectory planner during the optimization process for the first and second evaluation paths respectively.

Fig. 29

Total time of the trajectory tracking computed by the trajectory planner during the optimization process for the first evaluation path

Fig. 30

Total time of the trajectory tracking computed by the trajectory planner during the optimization process for the second evaluation path

Tables 12 and 13 show the summarized information of the total time of the trajectory tracking computed during the optimization process of the trajectory planner for the first and second evaluation paths respectively.

Table 12 Total time of the trajectory tracking computed by the trajectory planner for the first evaluation path

Table 13 Total time of the trajectory tracking computed by the trajectory planner for the second evaluation path

Tables 14 and 15 gather the percentage of the configuration parameters with respect to their maximum value for the magnitudes of the initial trajectories of the first and second evaluation paths respectively.

Table 14 Percentage of the configuration parameters with respect to their maximum value for the magnitudes of the initial trajectories of the first evaluation path

Table 15 Percentage of the configuration parameters with respect to their maximum value for the magnitudes of the initial trajectories of the second evaluation path

Tables 16 and 17 collect the percentage of the configuration parameters with respect to their maximum value for the magnitudes of the planned trajectories of the first and second evaluation paths respectively.

Table 16 Percentage of the configuration parameters with respect to their maximum value for the magnitudes of the planned trajectories of the first evaluation path

Table 17 Percentage of the configuration parameters with respect to their maximum value for the magnitudes of the planned trajectories of the second evaluation path

Tables 18 and 19 display the percentage of the energy (computed with Eq. 88) of the planned trajectory magnitudes with respect to the initial trajectories of the first and second evaluation paths respectively.

Table 18 Percentage of the energy (88) of the planned trajectory magnitudes with respect to the initial trajectories of the first evaluation path

Table 19 Percentage of the energy (88) of the planned trajectory magnitudes with respect to the initial trajectories of the second evaluation path.1

Tables 20 and 21 show the percentage of the energy (88) of the planned control commands references with respect to the initial trajectories of the first and second evaluation paths respectively.

Table 20 Percentage of the energy (88) of the planned control commands references with respect to the initial trajectories of the first evaluation path

Table 21 Percentage of the energy (88) of the planned control commands references with respect to the initial trajectories of the second evaluation path