Computing minimum-time trajectories for quadrotors via transverse coordinates

TL;DR

This paper introduces a new method using transverse coordinates to efficiently compute minimum-time trajectories for quadrotors in constrained environments, accounting for physical limitations.

Contribution

It presents a novel re-formulation of the trajectory optimization problem using transverse coordinates, enabling fixed horizon planning and easier constraint handling.

Findings

Successful numerical simulations in complex scenarios

Efficient handling of position constraints

Fixed horizon problem formulation

Abstract

In this paper we present a novel strategy to compute minimum-time trajectories for quadrotors. In particular, we consider the motion in constrained environments, taking into account the physical limitations of the vehicle. Instead of approaching the optimization problem in its standard time-parameterized formulation, the proposed strategy is based on an appealing re-formulation. Transverse coordinates, expressing the distance from a "reference" path, are used to parameterize the vehicle position and a spatial parameter is used as independent variable. This re-formulation allows us to (i) obtain a fixed horizon problem and (ii) easily formulate (even complex) position constraints. The effectiveness of the proposed strategy is proven by numerical computations on two different illustrative scenarios.

Equations

$$\dot{\mathbf{p}}$$

$$\dot{\text{v}}$$

$$\dot{\mathbf{\Phi }}$$

$$\dot{\mathbf{\omega }}$$

$$\dot{\mathbf{x}}(t)=f(\mathbf{x}(t),\mathbf{u}(t)),$$

$$\underset{\mathbf{x}(\cdot ),\mathbf{u}(\cdot )}{\min }$$

$$\dot{\mathbf{x}}(t)=f(\mathbf{x}(t),\mathbf{u}(t)),\mathbf{x}(0)={\mathbf{x}}_0\text{(dynamics)}$$

$$|{\omega }_i(t)|\le {\omega }_{i,max},i=1,2,3,\text{(angular rate)}$$

$$0<f_{min}\le f(t)\le f_{max}\text{(thrust)}$$

$$|\phi (t)|\le {\phi }_{max},|\theta (t)|\le {\theta }_{max}\text{(angles)}$$

$$c_{obs}(\mathbf{p}(t))\le 0\text{(position constraints)}$$

$$\bar{\mathbf{t}}(s)$$

$$\bar{\mathbf{n}}(s)$$

$$\bar{\mathbf{b}}(s)$$

$$\bar{R}^{\prime}_{SF}(s)=\bar{R}_{SF}(s)\left[\begin{array}[]{ccc}0&-\bar{k}(s)&0\\ \bar{k}(s)&0&-\bar{\tau}(s)\\ 0&\bar{\tau}(s)&0\\ \end{array}\right],$$

$$\pi (\mathbf{p}):=\text{arg}\underset{s\in {\mathbb{R}}_0^{+}}{\min }\Vert \mathbf{p}-{\bar{\mathbf{p}}}_r(s){\Vert }^2.$$

$${\varphi }_1(\mathbf{p})$$

$${\varphi }_2(\mathbf{p})$$

$$\mathbf{p}(t)={\bar{\mathbf{p}}}_r(s_r^t)+{\bar{R}}_{SF}(s_r^t){\mathbf{d}}_{SF}(t).$$

$$\text{v}(t)={\bar{\mathbf{p}}}_r^{\prime }(s_r^t){\dot{s}}_r^t+{\bar{R}}_{SF}^{\prime }(s_r^t){\dot{s}}_r^t{\mathbf{d}}_{SF}(t)+{\bar{R}}_{SF}(s_r^t){\dot{\mathbf{d}}}_{SF}(t).$$

$${\dot{s}}_r^t$$

$${\dot{w}}_1(t)$$

$${\dot{w}}_2(t)$$

$$\displaystyle\dot{\boldsymbol{\text{v}}}_{SF}=\bar{R}_{SF}^{T}\dot{\boldsymbol{\text{v}}}-\left[\begin{array}[]{ccc}0&-\bar{k}&0\\ \bar{k}&0&-\bar{\tau}\\ 0&\bar{\tau}&0\\ \end{array}\right]\dot{s}_{r}\boldsymbol{\text{v}}_{SF},$$

$${\dot{\mathbf{q}}}_S(t)=\tilde{f}({\mathbf{q}}_S(t),\mathbf{u}(t)),$$

$${\bar{\mathbf{q}}}_S^{\prime }(s)=\bar{f}({\bar{\mathbf{q}}}_S(s),\bar{\mathbf{u}}(s)),$$

$$\bar{f}({\bar{\mathbf{q}}}_S(s),\bar{\mathbf{u}}(s)):=\tilde{f}({\bar{\mathbf{q}}}_S(s),\bar{\mathbf{u}}(s))\frac{1-\bar{k}(s){\bar{w}}_1(s)}{{\bar{\text{v}}}_t(s)}.$$

$$\begin{split}\min_{\bar{\boldsymbol{q}}_{S}(\cdot),\bar{\boldsymbol{u}}(\cdot)}&\;\int_{0}^{L}\!\!\!\quad\frac{1-\bar{k}(s)\bar{w}_{1}(s)}{\bar{\text{v}}_{t}(s)}\;ds\\ \\ \!\!\text{subj. to}&\;\bar{\boldsymbol{q}}^{\prime}_{S}=\bar{f}(\bar{\boldsymbol{q}}_{S}(s),\bar{\boldsymbol{u}}(s)),\quad\bar{\boldsymbol{q}}_{S}(0)=\boldsymbol{q}_{S0}\;\text{\emph{(dynamics)}}\\ &\;\Big{(}\frac{\bar{\omega}_{i}(s)}{\omega_{i,max}}\Big{)}^{2}-1\leq 0,\;i=1,2,3\;\text{\emph{(angular rate)}}\\ &\;\Big{(}\frac{2\bar{f}(s)-(f_{max}+f_{min})}{(f_{max}-f_{min})}\Big{)}^{2}-1\leq 0\;\text{\emph{(thrust)}}\\ &\;\Big{(}\frac{\bar{\varphi}(s)}{\bar{\varphi}_{max}(s)}\Big{)}^{2}-1\leq 0\;\text{\emph{(roll angle)}}\\ &\;\Big{(}\frac{\bar{\theta}(s)}{\bar{\theta}_{max}(s)}\Big{)}^{2}-1\leq 0\;\text{\emph{(pitch angle)}}\\ &\;\bar{c}_{obs}(\bar{\boldsymbol{w}}(s))\leq 0\;\text{\emph{(position constraints).}}\end{split}$$

$$\displaystyle\Big{(}\frac{2\bar{w}_{i}(s)-(\bar{w}_{i,max}(s)+\bar{w}_{i,min}(s))}{(\bar{w}_{i,max}(s)-\bar{w}_{i,min}(s))}\Big{)}^{2}-1\leq 0,$$

$$\displaystyle\Big{(}\frac{||\bar{\boldsymbol{w}}(s)||}{\bar{r}_{obs}(s)}\Big{)}^{2}-1\leq 0,\quad\forall s\in[0,L],$$

$$\begin{array}{rl}\underset{\xi }{\min }& h(\xi ;\bar{k})+ϵb_{\nu }(\xi )\\ \text{subj. to}& {\bar{\mathbf{q}}}_S^{\prime }(s)=\bar{f}({\bar{\mathbf{q}}}_S(s),\bar{\mathbf{u}}(s)),{\bar{\mathbf{q}}}_S(0)={\mathbf{q}}_{S0}\end{array}$$

$$h(\xi ;\bar{k})$$

$$b_{\nu }(\xi )$$

$${\beta }_{\nu }(x)$$

$${\bar{\mathbf{q}}}_S^{\prime }(s)$$

$$\bar{\mathbf{u}}(s)$$

$$\begin{array}{rl}\underset{\xi }{\min }& g(\xi ;\bar{k}),\end{array}$$

Full text

I INTRODUCTION

Numerous applications involving quadrotors require them to move inside areas characterized by physical boundaries, obstacles and even tight space constraints (as e.g., urban environments) in order to accomplish their robotics tasks. Trajectory generation, a core step for physical task realization [1], becomes extremely challenging in this scenario. A physically realizable trajectory must satisfy (i) the (nonlinear) system dynamics, (ii) the physical limits of the vehicle, and (iii) the position constraints. Moreover, the computation of optimal (rather than just feasible) trajectories is more and more becoming a necessity to ensure an efficient accomplishment of the tasks, posing an additional challenge in the trajectory generation problem.

Quadrotor trajectory planning in constrained environments can be addressed using a reacting (e.g., [2, 3]) or a planning approach. The majority of the planning algorithms, such as [4], [5], [6], take advantage of the differential flatness property to avoid the integration of nonlinear differential equations, reduce the order of the problem and simplify the definition of position constraints. The problem is posed in the flat output space, where outputs are approximated as a combination of primitives, such as polynomial functions [4], B-splines [5], or convex functions [6]. The parameters of the primitives are the optimization variables. When dealing with obstacle dense environments, trajectory generation is often performed using a decoupled approach ([7], [8], [9]) aiming to avoid the difficult definition of spatial constraints and the high computational cost. In a first stage, a collision-free path is generated by sampling-based path planning algorithms, such as the Rapidly-exploring Random Tree (RRT) in [7, 8] or the Probabilistic Roadmap (PRM) in [9], and without the dynamics constraint. In a second stage, an optimal trajectory (satisfying the system dynamics) is generated from the collision free path. Optimization techniques such as [10], [4], [5] can be used at this stage. Differently from these works, the one in [6] considers a space-parameterized problem re-formulation, suitable for modeling complex flight scenarios, and thus avoids the decoupled approach. Finally, only few works do not consider the differential flatness hypothesis, such as [11]. Nevertheless, in [11] a discretized simple point-mass dynamics and approximated convex constraints are considered.

Our main contribution is the design of an optimization framework to generate feasible minimum-time quadrotor trajectories in geometrically constrained environments. In our problem setup (i) the vehicle nonlinear dynamics is directly included into the optimization problem (the differential flatness assumption is not required) and (ii) the vehicle position, which is an optimization variable, is not approximated via primitives. Similarly to [6], the minimum-time problem is formulated by using the path parameter as independent variable: a fixed horizon problem is obtained and even complex position constraints can be easily formulated. The optimization problem is solved combining the Projection Operator Newton method for Trajectory Optimization (PRONTO) [12] with a barrier function approach [13]. Similar problem setups and optimization methods are presented in [14] for two-wheeled mobile robots and in [15] for UAVs. In [14] a minimum-energy problem in a bi-dimensional space with obstacles is addressed by considering a standard formulation (i.e., with variables parameterized by the time). In [15] a virtual target approach (without position constraints) is presented using an arc-length parameterized transverse dynamics model. Differently from these works, we solve the optimization problem in the three-dimensional space and we define obstacle constraints by an arc-length parameterization. As an additional contribution, we present numerical computations on two challenging scenarios with respectively, a circular tube of variable section and a narrow corridor with an obstacle, as constrained spaces.

The paper is organized as follows. In Section II we present the standard formulation of the optimization problem we aim to solve. In Section III our trajectory generation strategy, based on an appealing reformulation of the problem, is described. Finally, in Section IV, we provide numerical computations discussing the minimum-time vehicle behavior.

II The quadrotor minimum-time problem

We first briefly introduce the quadrotor model used in the paper and then recall the standard problem formulation.

The quadrotor dynamics can be described by the so called vectored-thrust dynamical model, [16],

[TABLE]

where $\boldsymbol{p}=[p_{1}\;p_{2}\>p_{3}]^{T}$ is the vehicle position expressed in the inertial frame $\mathcal{F}_{i}$, $\boldsymbol{\text{v}}=[\text{v}_{1}\;\text{v}_{2}\;\text{v}_{3}]^{T}$ is the linear velocity expressed in $\mathcal{F}_{i}$, $\boldsymbol{\omega}=[\omega_{1}\;\omega_{2}\;\omega_{3}]^{T}$ is the angular rate expressed in the body frame $\mathcal{F}_{b}$ ($\Omega\in so(3)$ is such that, for $\boldsymbol{\beta}\in\mathbb{R}^{3},\Omega\boldsymbol{\beta}=\boldsymbol{\omega}\times\boldsymbol{\beta}$), and $J\in\mathbb{R}^{3\times 3}$ is the Jacobian matrix. The rotation matrix $R(\boldsymbol{\Phi})\in SO(3)$, mapping vectors in $\mathcal{F}_{b}$ into vectors in $\mathcal{F}_{i}$, is parameterized using roll-pitch-yaw angles, denoted by $\boldsymbol{\Phi}:=[\varphi\;\theta\;\psi]^{T}$. Furthermore, $m$ and $I$ are respectively the mass and the inertia matrix, $g$ is the gravity constant, and $\boldsymbol{e}_{3}=[0\;0\;1]^{T}$. The vehicle is controlled by the thrust $f$ and the torques $\boldsymbol{\gamma}$.

For the vehicle maneuvering, we use a cascade control scheme with an off-board position/attitude control loop and an on-board angular rate controller. Assuming that the virtual control input $\boldsymbol{\omega}$ is tracked by the on-board angular rate controller, we restrict our trajectory generation problem on the position/attitude subsystem (1-3), which can be written in state-space form as

[TABLE]

with state $\boldsymbol{x}=[\boldsymbol{p}^{T}\;\boldsymbol{\text{v}}^{T}\;\boldsymbol{\Phi}^{T}]^{T}$ and input $\boldsymbol{u}=[\boldsymbol{\omega}^{T}\;f]^{T}$.

We deal with the following optimal control problem:

[TABLE]

where $\omega_{i,max}$ are the bounds on the angular rate, $f_{min}$ and $f_{max}$ are lower an upper bounds on thrust, $\varphi_{max}$ and $\theta_{max}$ are the bounds on the roll and pitch angles respectivelly, and $c_{obs}:{\mathbb{R}}^{3}\rightarrow{\mathbb{R}}$ represents position constraints taking into account physical boundaries and obstacles. The position constraints may also represent GPS denied areas or spaces with limited communication. Angular rates have bounds, due to, for example, a limited range of the gyroscopes or to controller limitations. The vehicle thrust is also limited: quadrotor vehicles can only generate positive thrust and the maximum rotor speed is limited. Furthermore, constraints on angles are imposed into the optimization problem in order to avoid acrobatic vehicle configurations.

III Minimum-Time Trajectory Generation Strategy

In order to solve the minimum-time problem, we consider an equivalent formulation of (6) where a spatial parameter (instead of time) is the independent variable. The derivation of the equivalent problem formulation is based on three steps: (i) generation of a “reference” path, (ii) definition of the transverse dynamics, in which the vehicle position is parameterized by transverse coordinates (with respect to that path), and (iii) mapping of time-parameterized cost and constraints into arc-length parameterized ones. The PRONTO method, combined with a barrier function approach, is then used to numerically solve the optimization problem.

III-A Minimum-Time problem: equivalent formulation

The first step to convert the standard problem into the new formulation is the computation of a “reference” path $\bar{\boldsymbol{p}}_{r}(s)$, $\forall s\in[0,L]$, where $s$ is the arc-length of the path and $L$ is its total length. In the following, we denote the arc-length parameterized functions with a bar, and the derivative with respect to the arc-length with a prime, i.e., $\bar{\boldsymbol{p}}_{r}^{\prime}(s):=d\bar{\boldsymbol{p}}_{r}(s)/ds$. The “reference” path is computed as a $C^{\infty}$ geometric curve, e.g., using arctangent functions, as in our numerical computations. Note that $\bar{\boldsymbol{p}}_{r}(\cdot)$ is not required to satisfy the position constraints, and it can be suitably chosen.

With $\bar{\boldsymbol{p}}_{r}(\cdot)$ in hand, we re-write the system dynamics (5) as follows.

First, we design a change of coordinates from the inertial position $\boldsymbol{p}\in{\mathbb{R}}^{3}$ to the transverse coordinate vector $\boldsymbol{w}\in{\mathbb{R}}^{2}$, such that $\boldsymbol{w}=[w_{1}\;w_{2}]^{T}$. Let us consider the “reference” path $\bar{\boldsymbol{p}}_{r}(\cdot)$. A Serret-Frenet frame, whose origin has $\bar{\boldsymbol{p}}_{r}(s)$ as coordinates, can be defined $\forall s\in[0,L]$. In particular, the tangent, normal and bi-normal vectors, respectively $\bar{\boldsymbol{t}}(s),\bar{\boldsymbol{n}}(s),\bar{\boldsymbol{b}}(s)$, are defined, with components in the inertial frame, as

[TABLE]

where $\bar{k}(s):=\|\bar{\boldsymbol{p}}^{\prime\prime}_{r}(s)\|_{2}$ is the curvature of $\bar{\boldsymbol{p}}_{r}(\cdot)$ at $s$. Moreover, we define the rotation matrix $\bar{R}_{SF}:=[\bar{\boldsymbol{t}}\;\bar{\boldsymbol{n}}\;\bar{\boldsymbol{b}}]$ mapping vectors with components in the Serret-Frenet frame into vectors with components in the inertial frame. According to the Serret-Frenet formulas, the arc-length derivative of the Serret-Frenet rotation matrix is

[TABLE]

where $\bar{\tau}(s):=\bar{\boldsymbol{n}}(s)\;\bar{\boldsymbol{b}}^{\prime}(s)$ is the torsion of $\bar{\boldsymbol{p}}_{r}(\cdot)$ at $s$. Let us now consider the quadrotor center of mass with position $\boldsymbol{p}$. As depicted in Figure 1, its orthogonal projection on the “reference” path identifies a point with position $\bar{\boldsymbol{p}}_{r}(\pi(\boldsymbol{p}))$, where the function $\pi:{\mathbb{R}}_{0}^{+}\rightarrow{\mathbb{R}}_{0}^{+}$ is defined as

[TABLE]

Note that, the minimizing arc-length is unique provided that $\bar{\boldsymbol{p}}_{r}(\cdot)$ is locally a non-intersecting $C^{2}$ curve with non-vanishing $\bar{\boldsymbol{p}}^{\prime}_{r}(\cdot)$. By mapping $\boldsymbol{p}-\bar{\boldsymbol{p}}_{r}(\pi(\boldsymbol{p}))$ into a vector with components in the Serret-Frenet frame attached to $\bar{\boldsymbol{p}}_{r}(\pi(\boldsymbol{p}))$, we obtain $\boldsymbol{d}_{SF}:=R_{SF}(\pi(\boldsymbol{p}))^{T}(\boldsymbol{p}-\bar{\boldsymbol{p}}_{r}(\pi(\boldsymbol{p})))$. Moreover, noticing that the component related to the tangent vector is always zero by construction, we define the transverse coordinates as the second and third components of $\boldsymbol{d}_{SF}$. Thus $\boldsymbol{d}_{SF}:=[0\;w_{1}\;w_{2}]^{T}$. More formally, we define the locally invertible function $\phi:{\mathbb{R}}^{3}\rightarrow{\mathbb{R}}^{2}$, such that $\boldsymbol{w}=\phi(\boldsymbol{p})$, with components defined as

[TABLE]

Moreover, we denote by $\boldsymbol{\text{v}}_{SF}:=\bar{R}_{SF}(\pi(\boldsymbol{p}))^{T}\boldsymbol{\text{v}}$ the linear velocity of the quadrotor with components in the Serret-Frenet frame. In particular, $\boldsymbol{\text{v}}_{SF}=[\text{v}_{t}\;\text{v}_{n}\;\text{v}_{b}]^{T}$.

Second, we note that, considering the position function $\boldsymbol{p}(\cdot)$ with the time $t$ as independent variable, the function $s_{r}:=\pi\circ\boldsymbol{p}$ provides a change of variables from the time $t$ to the arc-length $s$. In particular, we denote by $s_{r}^{t}:=s_{r}(t)$ the function $s_{r}(\cdot)$ evaluated at the time $t$, and by $\dot{s}_{r}^{t}:=\dot{s}_{r}(t)$ the time derivative of $s_{r}(\cdot)$, evaluated at $t$. A generic arc-length function $\bar{\alpha}(\cdot)$ can thus be expressed as the time function $\bar{\alpha}(s_{r}(\cdot))$. Note that, the time derivative of $\bar{\alpha}(s_{r}(\cdot))$ is $\frac{d\bar{\alpha}(s_{r}(t))}{dt}=\bar{\alpha}^{\prime}(s_{r}^{t})\;\dot{s}_{r}^{t}$. Using the function $s_{r}:{\mathbb{R}}_{0}^{+}\mapsto{\mathbb{R}}_{0}^{+}$, the position of the quadrotor center of mass $\boldsymbol{p}(t)$, at time instant $t$, can be written as

[TABLE]

Differentiating (14) with respect to time, we get

[TABLE]

Multiplying both members of equation (15) by $\bar{R}^{T}_{SF}$, using (10), the definition of $\boldsymbol{\text{v}}_{SF}$, and $\bar{\boldsymbol{p}}^{\prime}_{r}(s_{r}^{t})=\bar{R}_{SF}(s_{r}^{t})\;[1\;0\;0]^{T}$, we get

[TABLE]

Furthermore, since we are using transverse coordinates to parameterize the vehicle position, we re-write equation (2) using $\boldsymbol{\text{v}}_{SF}$ instead of v. Differentiating both members of $\boldsymbol{\text{v}}(t)=\bar{R}_{SF}(s_{r}^{t})\boldsymbol{\text{v}}_{SF}(t)$ with respect to time, and using (2) and (10), we get

[TABLE]

with $\dot{\boldsymbol{\text{v}}}=(g\boldsymbol{e}_{3}-fm^{-1}R(\boldsymbol{\Phi})\boldsymbol{e}_{3})$. The dependence on $t$ and $s_{r}^{t}$ is omitted for simplicity. Equations (17), (18), (22) and (3) can be compactly written as

[TABLE]

with state $\boldsymbol{q}_{S}=[\boldsymbol{w}^{T}\;\boldsymbol{\text{v}}_{SF}^{T}\;\boldsymbol{\Phi}^{T}]^{T}$ and input $\boldsymbol{u}=[\boldsymbol{\omega}^{T}\;f]^{T}$.

Third and final, we write the dynamics (23) in terms of the arc-length $s$. By using the chain rule, we have $\dot{\boldsymbol{q}}_{S}(t)=\bar{\boldsymbol{q}}_{S}^{\prime}(s_{r}^{t})\dot{s}_{r}^{t}$. Moreover, we parameterize the state $\boldsymbol{q}_{S}$ and the input $\boldsymbol{u}$ using $s_{r}^{t}$, i.e., $\boldsymbol{q}_{S}(t)=\bar{\boldsymbol{q}}_{S}(s_{r}^{t})$ and $\boldsymbol{u}(t)=\bar{\boldsymbol{u}}(s_{r}^{t})$. Thus, equations (23) can be written as the transverse dynamics

[TABLE]

with the arc-length $s$ as independent variable and where

[TABLE]

Re-writing the time-parameterized cost and constraints of (6) into arc-length parameterized ones, the new formulation of the minimum-time problem, using the new set of coordinates $(\bar{\boldsymbol{q}}_{S},\bar{\boldsymbol{u}})$ and with $s$ as independent variable, is

[TABLE]

The constraints on the roll and pitch angles, the thrust and the angular rates are re-written by means of an $s$-parameterized equivalent form (with smooth functions). The position constraints $\bar{c}_{obs}(\bar{\boldsymbol{w}}(s))\leq 0$ have specific formulations according to the shape of the collision free region. Considering regions with rectangular or circular transversal sections, we get the following inequalities. For rectangular sections, the position constraints are

[TABLE]

$\forall s\in[0,L]$, $\forall i=1,2,$ where the non constant functions $\bar{w}_{i,min}(\cdot)$ and $\bar{w}_{i,max}(\cdot)$ identify the obstacle boundaries. For circular sections, the position constraint is

[TABLE]

where the non constant function $\bar{r}_{obs}(\cdot)$ identifies the circular boundary of the collision free region.

The new formulation (24) is equivalent to (6) since trajectories solving (24) can be mapped into trajectories solving (6). Using transverse coordinates, the formulation of position constraints is simpler than using the time-parameterized inertial frame position. Since collision free region boundaries $\bar{w}_{i,min}(\cdot)$, $\bar{w}_{i,max}(\cdot)$ and $\bar{r}_{obs}(\cdot)$ are arc-length functions, even complex spaces can be modelled.

III-B Numerical solution to the optimization problem

In order to solve problem (24), we use a combination of the PRojection Operator based Newton method for Trajectory Optimization (PRONTO) [12] with a barrier function approach [13]. We relax state-input constraints by adding them in the cost functional, i.e., given the state-input curve $\xi=(\bar{\boldsymbol{q}}_{S}(\cdot),\bar{\boldsymbol{u}}(\cdot))$, we consider the problem

[TABLE]

where

[TABLE]

and $c_{j}(\bar{\boldsymbol{q}}_{S}(s),\bar{\boldsymbol{u}}(s))$, j=1,..,7 denote the constraints in problem (24). The strategy to find a solution to (24) consists in iteratively solving problem (27) by reducing the parameters $\epsilon$ and $\nu$ at each iteration, and thus pushing the trajectory towards the constraint boundaries. Each instance of problem (27) is solved by means of the PRONTO algorithm. This is based on a properly designed projection operator $\mathcal{P}:\xi_{c}\rightarrow\xi$, mapping a state-control curve $\xi_{c}=(\bar{\boldsymbol{q}}_{c}(\cdot),\bar{\boldsymbol{u}}_{c}(\cdot))$ into a system trajectory $\xi$, by the nonlinear feedback system

[TABLE]

where the feedback gain $\bar{K}(\cdot)$ is designed by solving a suitable linear quadratic optimal control problem. The projection operator is used to convert the dynamically constrained optimization problem (27) into the unconstrained problem

[TABLE]

where $g(\xi;\bar{k})=h(\mathcal{P}(\xi);\bar{k})+\epsilon b_{\nu}(\mathcal{P}(\xi)).$ Then, using an (infinite dimensional) Newton descent method, a local minimizer of (29) is computed.

An initial trajectory, with a position matching the “reference” path, is used to initialize the algorithm. Given the current trajectory iterate $\xi_{i}$, the search direction $\zeta_{i}$ is obtained by solving a linear quadratic optimal control problem with cost $Dg(\xi_{i};\bar{k})\cdot\zeta+\frac{1}{2}D^{2}g(\xi_{i};\bar{k})(\zeta,\zeta)$, where $\zeta\mapsto Dg(\xi_{i};\bar{k})\cdot\zeta$ and $\zeta\mapsto D^{2}g(\xi_{i};\bar{k})(\zeta,\zeta)$ are respectively the first and second Fréchet differentials of the functional $g(\xi,\bar{k})$ at $\xi_{i}$. Then, the curve $\xi_{i}+\gamma_{i}\zeta_{i}$, where $\gamma_{i}$ is a step size obtained through a standard backtracking line search, is projected, by means of the projection operator, in order to get a new trajectory $\xi_{i+1}$. The strength of this approach is that the local minimizer of (29) is obtained as the limit of a sequence of trajectories, i.e., curves satisfying the dynamics. Furthermore, the feedback system (28), defining the projection operator, allows us to generate trajectories in a numerically stable manner.

IV Numerical computations

In this section, we present numerical computations to show the effectiveness of the proposed strategy.

We consider two different scenarios with a nano-quadrotor having mass $m=0.021\;\text{kg}$.

In the first scenario, the vehicle moves inside a tube with a $3$D center line and circular transversal sections getting smaller along it. Results are depicted in Figures 2 and 3. As regards the initial trajectory (dot-dashed green line), by choosing as “reference” path the 3D center-line of the tube, the collision free region is defined by constraint (26) where $\bar{r}_{obs}(\cdot)$ is depicted in Figure 2b (gray line). Furthermore, we set an initial constant velocity and a zero yaw angle. The remaining initial states and inputs are computed by tracking these positions and velocities. Note that, in real applications, the bounds of the collision free region should be restricted (with respect to the physical boundaries of the region) in order to consider the vehicle dimension and an imperfect tracking of the trajectory. Each intermediate optimal trajectory (dotted black line) is computed by iteratively solving problem (27) with decreasing values of $\epsilon$ and $\nu$. As regards the minimum-time trajectory (blue line), the maneuver is performed in $0.36\;\text{s}$, the optimal path touches the tube at arc-lengths $4.11\;\text{m}$, $9.35\;\text{m}$ and $11.88\;\text{m}$ (Figure 2b) and the velocity $\bar{\text{v}}_{t}$ (Figure 2c) reaches more than $7\;\text{m/s}$ at the end of the path. As regards inputs, while constraints on thrust (Figure 3d) are always active, roll and pitch rate (Figures 3a and 3b, respectively) are saturated after $s=1\;\text{m}$ and the yaw rate (Figure 3c) touches the constraint boundary only in the beginning of the maneuver.

In the second scenario, the vehicle moves from one room to another through a narrow corridor with a squared section with side length $0.5\;\text{m}$. Leaving the corridor, the vehicle encounters an obstacle and is forced to fly above it, having a final altitude $\bar{p}_{3}(L)\leq-1\;\text{m}$. In this scenario, as an additional requirement, we seek for a trajectory reaching the desired final state $\boldsymbol{\bar{q}}_{S}^{d}=[0\;-1.5\;0\;0\;0\;0\;0\;0]^{T}$. In order to fulfil this objective, we use a penalty function approach by adding the term $\frac{1}{2}(\boldsymbol{\bar{q}}_{S}^{d}-\boldsymbol{\bar{q}}_{S}(L))^{T}\rho I\;(\boldsymbol{\bar{q}}_{S}^{d}-\boldsymbol{\bar{q}}_{S}(L))$, where $\rho$ is a penalty parameter and $I$ is the identity $8\times 8$ matrix, in the cost functional of problem (24). Results are depicted in Figures 4 and 5. We choose as “reference” path an unfeasible curve on the $\bar{p}_{1}-\bar{p}_{2}$ plane and the collision free region is defined by constraint (25) where obstacle boundaries $\bar{w}_{i,min}(\cdot)$ and $\bar{w}_{i,max}(\cdot)$, $i=1,2$ are depicted in gray in Figures 4b and 4c. In particular, $\bar{w}_{2,min}$ is defined in order to take into account the presence of the obstacle that forces the vehicle to fly above it. An initial constant velocity and a zero yaw angle are considered and the remaining initial states and inputs are computed as in the first scenario. Note that, even if the initial trajectory is unfeasible, intermediate trajectories are feasible. As regards the minimum-time trajectory, the maneuver is performed in $0.5\;\text{s}$ and the path touches the constraint boundaries when the vehicle is inside the corridor (Figures 4b and 4c). The velocity $\bar{\text{v}}_{t}$ (Figure 4d) reaches a peak of $9\;\text{m/s}$ in the middle of the path and approaches zero at the end. As regards the inputs, only constraints on roll and pitch rates (Figures 5a, 5b, respectively) are always active. Furthermore, note that the desired final state is not exactly reached, since our penalty function approach only guarantees approximated solutions. An ad-hoc strategy should be designed to exactly reach the desired final state.

V Conclusion

In this paper, we have presented a strategy to compute minimum-time trajectories for quadrotors in constrained spaces. Our approach consists of: (i) generating a “reference” path, (ii) expressing the vehicle dynamics using transverse coordinates, and (iii) re-defining constraints in the new coordinates. We obtain a re-formulation of the problem, which we solve by combining the PRONTO algorithm with a barrier function approach. Numerical computations on two challenging scenarios prove the effectiveness of the strategy. As a future work, we aim to compare our numerical results with a theoretical analysis on particular scenarios.