# Perception-Aware Perching on Powerlines with Multirotors

J. L. Paneque,1 J.R. Martínez-de Dios,1 A. Ollero,1 D. Hanover,2 S. Sun,2 A.
Romero,2 and D. Scaramuzza2 Manuscript received: September, 9, 2021; Revised
November, 10, 2021; Accepted January, 4, 2022.This paper was recommended for
publication by Editor P. Ponds upon evaluation of the Associate Editor and
Reviewers’ comments. This work was supported by the European Union’s Horizon
2020 Research and Innovation Programme under grant agreement No. 871479
(AERIAL-CORE), by the European Research Council (ERC) under grant agreements
No. 788247 (GRIFFIN) and No. 864042 (AGILEFLIGHT), by the National Centre of
Competence in Research (NCCR) Robotics through the Swiss National Science
Foundation (SNSF), and by the Spanish Ministerio de Universidades FPU Program.
1J. L. Paneque, J.R. Martínez-de Dios and A. Ollero are with the GRVC Robotics
Lab Sevilla, Universidad de Sevilla, Seville, Spain (email<EMAIL_ADDRESS><EMAIL_ADDRESS>aollero@us.es)2D. Hanover, S. Sun, A. Romero and D. Scaramuzza
are with the Robotics and Perception Group, Department of Informatics,
University of Zurich, and Department of Neuroinformatics, University of Zurich
and ETH Zurich, Zürich, Switzerland (email<EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS>davide.scaramuzza@ieee.org)Digital Object
Identifier (DOI): doi.org/10.1109/LRA.2022.3145514

###### Abstract

Multirotor aerial robots are becoming widely used for the inspection of
powerlines. To enable continuous, robust inspection without human
intervention, the robots must be able to perch on the powerlines to recharge
their batteries. Highly versatile perching capabilities are necessary to adapt
to the variety of configurations and constraints that are present in real
powerline systems. This paper presents a novel perching trajectory generation
framework that computes perception-aware, collision-free, and dynamically-
feasible maneuvers to guide the robot to the desired final state. Trajectory
generation is achieved via solving a Nonlinear Programming problem using the
Primal-Dual Interior Point method. The problem considers the full dynamic
model of the robot down to its single rotor thrusts and minimizes the final
pose and velocity errors while avoiding collisions and maximizing the
visibility of the powerline during the maneuver. The generated maneuvers
consider both the perching and the posterior recovery trajectories. The
framework adopts costs and constraints defined by efficient mathematical
representations of powerlines, enabling online onboard execution in resource-
constrained hardware. The method is validated on-board an agile quadrotor
conducting powerline inspection and various perching maneuvers with final
pitch values of up to 180°.

20cm(0.5cm,27cm) © 2022 IEEE. Personal use of this material is permitted.
Permission from IEEE must be obtained for all other uses, in any current or
future media, including reprinting/republishing this material for advertising
or promotional purposes, creating new collective works, for resale or
redistribution to servers or lists, or reuse of any copyrighted component of
this work in other works.

## Supplementary Material

Video: https://youtu.be/JsPavnsfpbk

Code: github.com/grvcPerception/pa_powerline_perching

##  I Introduction

The use of multicopters in the inspection of hazardous industrial environments
(e.g., nuclear plants, steel mills, or power lines) provides significant
opportunity to drastically reduce the risk of human injury on the job-site.
The US power system consists of nearly 160,000 miles of high-voltage power
lines, and millions of low-voltage power lines and distribution transformers,
which connect 145 million customers [1]. Inspections of this magnitude require
extensive manpower and work hours in highly dangerous environments. Leveraging
robust autonomous robots for inspection of infrastructures could improve
throughput of these inspections, thus reducing the possibility of failure.

In order to enable these opportunities, multicopters must be able to operate
in uncertain highly-cluttered environments, varying environmental conditions,
and with limited onboard energy. The multicopter is then responsible for
estimating its state via onboard sensors, calculating agile trajectories which
maximize inspection coverage, and executing dynamic maneuvers near crowded,
safety critical infrastructures. The powerline inspection task offers the
possibility of perching directly on the powerlines to recharge onboard battery
systems via wireless charging [2]. This has the potential to improve
efficiency of powerline inspection drones over manned inspection, but requires
the unmanned system to perch on the line. The challenge is then to design
algorithms which can identify powerlines and relevant obstacles in the
observable space, plan a trajectory that satisfies the dynamic constraints of
the multirotor and avoids obstacles while keeping the goal point in view (see
Fig. 1), and execute the trajectory in a potentially windy or rainy
environment.

Figure 1: A quadrotor performing a perception-aware perching maneuver,
maximizing the visibility of the line during the whole trajectory. The
maneuver starts at the bottom-right corner of the image (S), and progresses
first upwards to adjust its orientation, and then towards the objective line
(E).

Most systems perch on vertical walls by directing the robot towards them and
adjusting their angle during the maneuver [3, 4]. Existing systems for
perching on cables rely on approaching while hovering the objective line
either from the top [5] or the bottom [6] assuming there is enough space to do
so. Actual powerlines can come in many different configurations where this may
not be possible, necessitating that any planning algorithm must be able to
account for these configurations directly and plan accordingly. Simply
installing multiple perching devices will not suffice because it limits the
weight and efficiency of the platform.

###  I-A Contribution

This paper presents a novel perching trajectory generation framework for
powerlines, which produces highly versatile, agile, collision-free, and
dynamically-feasible maneuvers to guide the robot to a desired pose at zero
velocity while maximizing the visibility of the objective powerline. It is
based on a Nonlinear Programming (NLP) optimization problem that uses a
nonlinear quadrotor model formulated down to the rotor thrust level. The NLP
uses a simplified mathematical representation that efficiently represents the
powerlines (with negligible errors) as concatenations of segments and the
robot as an ellipsoid with three different radii.

The paper has four main contributions: 1) a highly-versatile perception-aware
agile perching trajectory generation method based on NLP; 2) a general
mathematical modeling for collision avoidance and perception awareness near
powerlines; 3) experimental validation in different agile maneuvers including
extreme 180° perching; and 4) the developed code of the trajectory generator,
which is released to the robotics community. We demonstrate the efficacy of
the proposed method onboard a quadrotor, first using the mathematical modeling
inside an onboard Nonlinear Model Predictive Controller (NMPC) to perform
inspection flights, and then using the developed framework to generate
perching maneuvers in three different lines, which were followed with final
pitch values of 20°, 80°, and 180°.

The paper is organized as follows: Section II summarizes the main works in the
topics addressed in the paper. Section III presents the problem formulation
for powerline perching. Section IV describes the proposed mathematical
modeling. Section V presents the method for perching trajectory generation.
Section VI provides an experimental validation of the developed work. Finally,
Section VII concludes the paper and highlights the main future research steps.

##  II Related Work

Prior works on multirotor perching have usually focused on the problem of
agile perching on walls. First, authors in [3] performed perching trajectories
by compounding multiple linear control modes that did not guarantee the
feasibility of the maneuver. Later, the work in [4] addressed this problem by
planning for dynamically feasible maneuvers before their execution, which is
also the case in the proposed method. Other works on perching on walls have
usually focused on the design of the perching mechanism [7, 8]. Recently, a
visual perching for walls was presented in [9], were the authors use a
combination of Apriltags and Visual Inertial Odometry to perch on walls
without a Motion Capture System. All these systems usually attach on walls by
colliding with them with some final velocity, while the proposed one reaches
the perching state at zero (or desired) velocity. On the other hand, works for
multirotor perching on cylinders (which can include powerlines) are usually
limited to reaching the desired spot at a hover state and attaching to the
cylinder using a gripper [6], [10]. A heterogeneous perching platform is
proposed on [5], which can rest or perch in a variety of different situations,
provided they are reached from above. For powerlines this is not always the
case, since they appear in many different configurations where hanging from
the upmost line can lead to touching the others. Recently, [11] presented a
quadrotor with upside-down perching capabilities by using bi-directional
thrusts. In contrast, the proposed system is able to generate perching
trajectories that take multirotors to any (feasible) desired perching state,
including upside-down, without the use of bi-directional thrusts.

During perching maneuvers, it is important to keep visibility of the final
objective, either if it is a landing area, a cylinder, or a powerline.
Traditional methods such as Image-Based Visual Servoing [6] enforce this
naturally by formulating the control law in the image space coordinates.
However, aggressive perching maneuvers may not always have the final spot
inside its Field of View (FOV), especially if the robot’s camera is not
located at the perching mechanism. Authors in [12] presented a perception-
aware NMPC for multirotors which uses additional error terms in its cost
function to keep visibility of a desired object while tracking a trajectory.
The work in [13] then proposed to keep different targets inside the
multirotor’s FOV by formulating their visibility as constraints inside an NMPC
controller. Later, authors in [14] developed a probabilistic constraint to
keep the number of successfully matched visual landmarks over a minimum
threshold during a flight, including the multirotor’s gimbal in the modeled
dynamics of their NMPC. We take inspiration in these works and include
perception awareness in the generated perching trajectories by formulating
novel costs and constraints designed for lines and segments instead of point
landmarks, so their perception can be considered in the computation of the
perching maneuver.

##  III Problem Formulation

The objective of powerline perching is to guide an aerial robot to a desired
final pose with zero velocity, where it can grip to a powerline. This has to
be done while avoiding collisions and maximizing the visibility of the
powerline during the trajectory. The basic scenario is composed of a set of
powerlines, not necessarily parallel, with several tens of meters of length
and at a certain height.

Powerlines follow catenary equations, whose use for costs and constraints
formulation in NLP systems would result in very inefficient implementations.
In our approach, to overcome this issue, we adopt a mathematical model that
approximates catenary shapes as concatenations of segments. Segments can be
very efficiently integrated in NLPs both for measuring robot-powerline
distances (to ensure collision-free maneuvers) and also for estimating the
powerline visibility from the robot camera. Powerlines can be represented by
several segments to provide an accurate representation, and there are already
algorithms to perform the approximation [15]. For instance, we measured that a
real powerline of 185m could be modeled by 15 segments with a mean length of
12m and a mean error of 1’5 cm with respect to it. Moreover, an average
perching maneuver will only involve 1 or 2 of these segments per line.

While perching maneuvers usually end when the robot reaches the objective
pose, it may happen that the perching device fails to attach to the powerline.
In that case, the system must be able to recover to a safe state, while still
avoiding the powerlines. Our NLP framework is used to also compute this
recovery trajectory, appended to the perching maneuver.

##  IV Mathematical modeling

###  IV-A Nomenclature

In this work, we follow standard conventions and denote scalar magnitudes with
lowercase $s$, vectors with bold lowercase $\mathbf{v}$, and matrices with
bold uppercase $\mathbf{M}$. We also make use of different reference frames,
all defined with uppercase $F$ and with an orthonormal basis
$\\{\mathbf{x}_{F},\mathbf{y}_{F},\mathbf{z}_{F}\\}$.

We represent translations between two coordinate frames as vectors
$\mathbf{p}_{F_{1}F_{2}}\in\mathbb{R}^{3}$, such that a vector
$\mathbf{v}_{F_{2}}\in\mathbb{R}^{3}$ is expressed in $F_{1}$ as:
$\mathbf{v}_{F_{1}}=\mathbf{p}_{F_{1}F_{2}}+\mathbf{v}_{F_{2}}$. For
rotations, we use unit quaternions $\mathbf{q}\in\,$SO(3), which can be
expressed in different frames as
$\mathbf{q}_{F_{1}}=\mathbf{q}_{F_{1}F_{2}}\odot\mathbf{q}_{F_{2}}$, where
$\odot$ denotes the Hamilton product between two quaternions. Finally, we
define the rotation of a vector $\mathbf{v}\in\mathbb{R}^{3}$ by a quaternion
with the following abuse of notation:
$\mathbf{q}\odot\mathbf{v}\coloneqq\mathbf{q}\odot\left[0,\mathbf{v}\right]^{T}$.

###  IV-B Multirotor Dynamics Model

Similarly to [16, 13], we model our multirotor robot as a rigid body of mass
$m$ and diagonal moment of inertia matrix $\mathbf{J}$, with nominal dynamics
$\dot{\mathbf{x}}$ down to their second order derivatives. The robot is
actuated by the thrusts $\boldsymbol{\gamma}\in\mathbb{R}^{4}$ of four
individually-controllable rotors, i.e.,
$\boldsymbol{\gamma}=\left[\gamma_{1},\gamma_{2},\gamma_{3},\gamma_{4}\right]^{T}$.
Typically, the individual rotor thrusts $\gamma_{i}$ are used as the control
inputs of the dynamic system and then are translated into desired rotational
speeds for the motors using a simple algebraic relation [17]. However, the
rotors actually behave as a first-order system with a time constant of several
ms, which means they cannot change their thrust instantaneously as demanded by
the controller. This effect is of high importance when generating perching
trajectories, which demand fast deceleration and rotation before the end of
the trajectory. Assuming instantaneous thrust dynamics potentially leads to
generating dynamically unfeasible maneuvers that cannot be followed by the
multirotor. To solve this, we model the inputs of the system as the desired
constant thrust derivatives $\boldsymbol{u}\in\mathbb{R}^{4}$ and include the
thrusts $\boldsymbol{\gamma}$ as part of the state of the system, similarly to
[13]. This ensures continuity in the required actuations and allows to include
the physical limits of the rotor angular accelerations and decelerations in
the NLP framework.

The 17-dimensional robot state space is then defined as:

$\dot{\mathbf{x}}=\begin{bmatrix}\dot{\mathbf{p}}_{WB}\\\
\dot{\mathbf{q}}_{WB}\\\ \dot{\mathbf{v}}_{W}\\\
\dot{\boldsymbol{\omega}}_{B}\\\
\dot{\boldsymbol{\gamma}}\end{bmatrix}=\begin{bmatrix}\mathbf{v}_{W}\\\
\mathbf{q}_{WB}\odot\left[0,{\boldsymbol{\omega}_{B}}^{T}/2\right]^{T}\\\
\frac{1}{m}\mathbf{q}_{WB}\odot\mathbf{\Gamma}_{B}+\mathbf{g}_{W}\\\
\mathbf{J}^{-1}\left(\mathbf{M}\boldsymbol{\gamma}-\boldsymbol{\omega}_{B}\times\mathbf{J}\boldsymbol{\omega}_{B}\right)\\\
\boldsymbol{u}\end{bmatrix}\in\mathbb{R}^{17},$ (1)

where $\textbf{p}_{WB}$ and $\textbf{q}_{WB}$ are the position and orientation
of the robot’s body frame $B$ w.r.t. the world frame $W$, and
${\mathbf{v}}_{W}$ and ${\boldsymbol{\omega}}_{B}$ $\in\mathbb{R}^{3}$ are the
linear and angular velocities of the multirotor robot, measured in global and
body axes of the robot respectively. Vector $\mathbf{g}_{W}\in\mathbb{R}^{3}$
denotes the acceleration due to gravity in global axes. The vector
$\mathbf{\Gamma}_{B}\in\mathbb{R}^{3}$ encodes the collective thrust of the
motors in the body axes, where in our case all 4 motors are directed to
$B_{z}$. Finally, $\mathbf{M}\in\mathbb{R}^{3\times 4}$ is the thrust
allocation matrix that converts the current rotor thrusts into body torques in
$B$:

$\mathbf{\Gamma}_{B}=\begin{bmatrix}0\\\ 0\\\
\sum\boldsymbol{\gamma}\end{bmatrix}\quad\mathbf{M}=\begin{bmatrix}\mathbf{r_{y}}^{T}\\\
\mathbf{-r_{x}}^{T}\\\ \kappa\mathbf{r_{d}}^{T}\end{bmatrix},$ (2)

where $\mathbf{r_{x}}$ and $\mathbf{r_{y}}\in\mathbb{R}^{4}$ are the rotor
displacements in $B_{x}$ and $B_{y}$, $\kappa$ is the rotor drag torque
constant, and $\mathbf{r_{d}}\in{\\{-1,1\\}}^{4}$ are the individual rotor
spin directions, where ${r_{d}}_{i}=-1$ for counter-clockwise direction and
${r_{d}}_{i}=1$, otherwise.

###  IV-C Segment collision avoidance

First, the robot-powerline collision is modeled assuming there is only one
straight powerline. Then, the model is extended to powerlines composed of
several segments.

Let the robot’s body be represented as an ellipsoid with principal axes
$\\{B_{x},B_{y},B_{z}\\}$ and principal radii $\boldsymbol{\delta}$. Assume
there is only one straight powerline whose radius is summed in
$\boldsymbol{\delta}$. The parametric equation of the line is given by
$\mathbf{o}_{W}+\tau\mathbf{l}_{W}$, where $\textbf{o}_{W}$ and
$\textbf{l}_{W}\in\mathbb{R}^{3}$ are the origin and direction vectors of the
line, and $\tau$ is a parameter. We can transform the line to the body frame
$B$ and scale it with $\boldsymbol{\delta}_{B}$:

$\mathbf{o}_{\breve{B}}=\mathbf{\Delta}_{B}\mathbf{q}_{BW}\odot\left(\mathbf{o}_{W}-\mathbf{p}_{WB}\right),\,\mathbf{l}_{\breve{B}}=\mathbf{\Delta}_{B}\mathbf{q}_{BW}\odot\mathbf{l}_{W},$
(3)

where $\boldsymbol{\Delta}_{B}$ =
$\operatorname{diag}\left(1/\\{\delta_{x},\delta_{y},\delta_{z}\\}\right)$.
Note that if $\boldsymbol{\delta}$ is defined in a different frame than $B$,
(3) can be rearranged using further transformations until
$\textbf{o}_{W},\textbf{l}_{W}$ are in its same frame.

Now that the line lies in the scaled reference frame of the ellipsoid,
ensuring that there is no intersection between them is equal to proving that
the distance from the line to the origin of that reference frame is higher
than 1. The squared point-line distance formula from the origin of $\breve{B}$
gives:

$\frac{\left\lVert\mathbf{o}_{\breve{B}}\times\mathbf{l}_{\breve{B}}\right\rVert^{2}}{\left\lVert\mathbf{l}_{\breve{B}}\right\rVert^{2}}>1,$
(4)

which can finally be simplified by using
$\left\lVert\mathbf{a}\times\mathbf{b}\right\rVert^{2}=\left\lVert\mathbf{a}\right\rVert^{2}\left\lVert\mathbf{b}\right\rVert^{2}-(\mathbf{a}\cdot\mathbf{b})^{2}$:

$\left(\left\lVert\mathbf{o}_{\breve{B}}\right\rVert^{2}-1\right)\left\lVert\mathbf{l}_{\breve{B}}\right\rVert^{2}-(\mathbf{o}_{\breve{B}}\cdot\mathbf{l}_{\breve{B}})^{2}>0$
(5)

When working with real powerlines, we need to use more than one straight
segment to approximate the curved shape of the powerline in the maneuver’s
surroundings. In that case, the collision constraint (5) could be activated
outside of its corresponding segment and interfere with the perching maneuver.
Thus, we need to extend this constraint such that it is not activated outside
of its segment’s area of effect.

The minimum value of (5) is reached whenever
$\mathbf{o}_{\breve{B}}=\mathbf{0}$ or
$\mathbf{o}_{\breve{B}}\parallel\mathbf{l}_{\breve{B}}$. In these cases, the
value will be $-\left\lVert\mathbf{l}_{\breve{B}}\right\rVert^{2}$. In (3),
$\mathbf{l}_{\breve{B}}$ is a matrix multiplied by a rotated unit direction
vector. The maximum possible value of
$\left\lVert\mathbf{l}_{\breve{B}}\right\rVert$ appears when
$\mathbf{q}_{BW}\odot\mathbf{l}_{W}$ is aligned with the principal eigenvector
of $\boldsymbol{\Delta}_{B}$, and corresponds to
$\lambda_{1}(\boldsymbol{\Delta}_{B})$, which is the maximum eigenvalue of
$\boldsymbol{\Delta}_{{B}}$. Thus, we can force the collision avoidance
function (5) to be always positive by summing
$\lambda^{2}_{1}(\boldsymbol{\Delta}_{B})$ to it. We define the following
function to add that value whenever the robot is outside of the segment’s
surroundings:

${k(\mathbf{x})}=\lambda^{2}_{1}(\boldsymbol{\Delta}_{B})\operatorname{sigm}((\left\lVert\mathbf{p}_{WB}-\mathbf{o}_{W}\right\rVert))),$
(6)

where $\left\lVert\mathbf{p}_{WB}-\mathbf{o}_{W}\right\rVert$ is the distance
from the robot to the segment’s center, and
$\operatorname{sigm}\left(\cdot\right)$ can be any sigmoid function that is
scaled and translated such that $k$ is 0 when this distance is lower than half
the segment’s length plus the highest radii in $\mathbf{\delta}_{B}$, and is
$\lambda^{2}_{1}(\boldsymbol{\Delta}_{B})$ otherwise, since then the robot
will never collide with the segment, even when intersecting with its
corresponding line. We chose the $\arctan$ function since it is available in
two of the main NLP code-generation frameworks [18, 19], while others such as
$\tanh$ are only available in [19]. Finally, summing (6) into (5) ensures that
the resulting collision avoidance constraint is only activated in the
surroundings of the segment:

$h_{ca}\left(\textbf{x}\right)\coloneqq\left(\left\lVert\mathbf{o}_{\breve{B}}\right\rVert^{2}-1\right)\left\lVert\mathbf{l}_{\breve{B}}\right\rVert^{2}-(\mathbf{o}_{\breve{B}}\cdot\mathbf{l}_{\breve{B}})^{2}+k(\mathbf{x})>0$
(7)

###  IV-D Segment perception awareness

Following the previous approach, we first assume there is only one straight
powerline in the scenario, then extend to the segment-based case. Let $C$ be
the reference frame of a camera mounted on the robot. The position and
orientation of $C$ are given by $\textbf{p}_{WC}$ and $\textbf{q}_{WC}$, which
are computed from the robot’s current body pose and a fixed transformation
$T_{BC}=\\{\textbf{p}_{BC},\textbf{q}_{BC}\\}$. A line is expressed in frame
$C$ as:

$\mathbf{o}_{C}=\mathbf{q}_{CW}\odot\left(\mathbf{o}_{W}-\mathbf{p}_{WC}\right),\quad\mathbf{l}_{C}=\mathbf{q}_{CW}\odot\mathbf{l}_{W}$
(8)

We then redefine the line by its Plücker coordinates, i.e. the normal of the
plane that intersects with it and the origin, and its direction vector:
$\\{\mathbf{n}_{C},\mathbf{l}_{C}\\}$, with
$\mathbf{n}_{C}=\mathbf{o}_{C}\times\mathbf{l}_{C}$.

Assume a classical pinhole camera model with parameters
$\\{f_{x},f_{y},c_{x},c_{y}\\}$. For brevity, assume the pixel coordinates are
centered at the optical axis (i.e., $c_{x}=c_{y}=0$). The transformation of
the direction vector onto the 3D image frame $I$ is given by
$\mathbf{l}_{I}=\mathbf{K}_{P}\mathbf{l}_{C}$, with
$\mathbf{K}_{P}=\operatorname{diag}\left(f_{x},f_{y},1\right)\in\mathbb{R}^{3\times
3}$ being the intrinsic camera matrix. Similarly, the vector $\mathbf{n}_{C}$
is transformed onto the image coordinates as $\mathbf{K}_{L}\mathbf{n}_{C}$,
with
$\mathbf{K}_{L}=\operatorname{diag}\left(f_{y},f_{x},f_{x}f_{y}\right)\in\mathbb{R}^{3\times
3}$. The point-line reprojection error for a given 2D image point $\mathbf{m}$
is [20]:

$\widetilde{r}\left(\textbf{x}\right)=\frac{\underline{\mathbf{m}}^{T}\mathbf{n}_{I}}{\sqrt{n_{I,x}^{2}+n_{I,y}^{2}}},$
(9)

where $\underline{\mathbf{m}}\in\mathbb{R}^{3}$ is the 2D point in homogeneous
coordinates.

As stated in Section II, it is convenient to keep the tracked objects (either
points, lines, or other shapes) as close as possible to the center of the
image. This allows the robot to focus on such objects and avoid losing track
of them, potentially improving the accuracy of the object’s localization
overtime (which is especially important for perching maneuvers and inspection
tasks). We can achieve this by choosing
$\underline{\mathbf{m}}^{T}=\mathbf{e}_{z}=\left[0,0,1\right]^{T}$ and
minimizing (9) for it.

However, there are two ways in which this function can be driven to zero: by
having the line centered in front of the camera, and by doing so behind the
camera. The second case is undesirable, since for a single pinhole camera this
means the system may not see the line. We need an additional constraint to
ensure the line is centered in front of the camera. To define this constraint,
we first obtain two new vectors:

$\mathbf{p}^{2D}_{I}=\mathbf{n}_{I}\times\left(\mathbf{e}_{z}\times\mathbf{n}_{I}\right),\quad\mathbf{d}^{3D}_{I}=\mathbf{l}_{I}\times\mathbf{n}_{I}$
(10)

The vector $\mathbf{p}^{2D}_{I}\in\mathbb{R}^{3}$ is directed to the closest
point of the line from the center of the image when the line is in 2D
normalized image coordinates (Fig. 2). Conversely, the vector
$\mathbf{d}^{3D}_{I}\in\mathbb{R}^{3}$ is the closest point from the line to
the origin of $I$ when the line is in 3D unnormalized image coordinates.

Figure 2: Visualization of proposed mathematical modeling for (9) and (10).

As stated before, we are interested in keeping the 2D line as close as
possible to the center of the image, which is the same as keeping
$\mathbf{p}^{2D}_{I}$ as parallel as possible to $\mathbf{e}_{z}$. Since the
vector $\mathbf{p}^{2D}_{I}$ marks where is the nearest point of the line from
the center of the image, if we obtain its intersection with the 3D line we can
recover its sign and force that is positive, thus having the line in front of
the camera when minimizing (9). The result of solving such intersection is:

$\mathbf{p}^{3D}_{I}=\frac{\mathbf{p}^{2D}_{I}}{d^{3D}_{I,z}},\quad{p}^{3D}_{I,z}=\frac{n_{I,x}^{2}+n_{I,y}^{2}}{d^{3D}_{I,z}}$
(11)

Note that the sign of ${p}^{3D}_{I,z}$ is determined by the sign of
$d^{3D}_{I,z}$ as its numerator will always be $\geq 0$. Thus, forcing the
line to be centered in front of the camera is equivalent to forcing the
following line cheirality (i.e., side) constraint:

$h_{lc}\left(\textbf{x}\right)\coloneqq{d}^{3D}_{I,z}>0$ (12)

We now extend the given formulation to work with segments, by defining a third
constraint that is complementary to (12). Let $\mathbf{e1}_{I}$ and
$\mathbf{e2}_{I}\in\mathbb{R}^{3}$ be the two endpoints of the inspected
segment in the 3D image coordinates. These points lie in the same line as
$\mathbf{p}^{3D}_{I}$, so the dot product between
$\left(\mathbf{p}^{3D}_{I}-\mathbf{e1}_{I}\right)$ and
$\left(\mathbf{p}^{3D}_{I}-\mathbf{e2}_{I}\right)$ is negative whenever
$\mathbf{p}^{3D}_{I}$ is located between $\mathbf{e1}_{I}$ and
$\mathbf{e2}_{I}$. This serves to create a constraint to keep
$\mathbf{p}^{3D}_{I}$ between the endpoints of the segment. If the line is
centered, this means at least half of the image will contain the segment. The
proposed segment visibility constraint is thus formulated as:

$h_{sv}\left(\textbf{x}\right)\coloneqq(-1)\left(\mathbf{p}^{3D}_{I}-\mathbf{e1}_{I}\right)\cdot\left(\mathbf{p}^{3D}_{I}-\mathbf{e2}_{I}\right)>0$
(13)

##  V Perching Trajectory Generation

###  V-A Optimization Problem Formulation

We model the perching maneuver generation as a discrete-time multiple-shooting
NLP problem sampled in $N$ shooting points over a non-fixed time horizon $T$,
which is also an optimization variable of the problem:

$\displaystyle\min_{\begin{matrix}\boldsymbol{u}_{0}...\boldsymbol{u}_{N-1}\\\
T\end{matrix}}\quad$
$\displaystyle\sum_{k=0}^{N}\left\lVert\mathbf{\widetilde{y}}_{k}\right\rVert^{2}_{\mathbf{Q}_{k}}$
(14a) s.t. $\displaystyle\mathbf{x}_{0}=\mathbf{x}_{init}$ (14b)
$\displaystyle T_{min}\leq T\leq T_{max}$ (14c)
$\displaystyle\mathbf{x}_{k+1}=\textbf{f}\left(\mathbf{x}_{k},\boldsymbol{u}_{k}\right)$
$\displaystyle\forall k\in\left[0,N-1\right]$ (14d) $\displaystyle
z_{min}\leq{p}_{WB,z}$ $\displaystyle\forall k\in\left[0,N\right]$ (14e)
$\displaystyle 0\leq\boldsymbol{\gamma}\leq\gamma_{max}$ $\displaystyle\forall
k\in\left[0,N\right]$ (14f) $\displaystyle u_{min}\leq\boldsymbol{u}\leq
u_{max}$ $\displaystyle\forall k\in\left[0,N-1\right]$ (14g) $\displaystyle
0<h_{lc}\left(\textbf{x}_{k}\right)$ $\displaystyle\forall
k\in\left[0,N\right]$ (14h) $\displaystyle
0<h_{sv}\left(\textbf{x}_{k}\right)$ $\displaystyle\forall
k\in\left[0,N\right]$ (14i) $\displaystyle 0\leq
h_{ca,i}\left(\textbf{x}_{k}\right)$ $\displaystyle\begin{matrix}\forall
k\in\left[0,N\right]\\\ \forall i\in\left[0,N_{L}-1\right]\end{matrix}$ (14j)

The problem (14) is built as follows: (14a) is the cost function to minimize,
including final and running terms; (14c) are the limits of the total maneuver
time $T$; (14d) are the dynamics of the system (see next paragraph); (14e) is
the allowed minimum height; (14f),(14g) are the constraints for the motor
thrusts and their derivatives; (14h),(14i) are the line cheirality (12) and
segment visibility (13) constraints for the objective line; and (14j) are the
line avoidance constraints defined by (5) and (6) for all the present
segments.

We implement the variable time horizon by modeling the system dynamics (14d)
using a Runge-Kutta4 integration of the state space, scaling its derivative
(1) by the total time $T$ and using an integration step of $1/N$ seconds.
Since problems where the total maneuver time is an optimization variable
suffer from bad linearization characteristics, we chose the ForcesPRO
framework [21] with [22] as the NLP solver, which embedded a linear system
solver with high numerical stability. Convergence was typically achieved
between 100 and 1000 iterations of its Nonlinear Primal-Dual Interior-Point
method, depending on the complexity of the required maneuver. This was a
feasible requirement since each maneuver is only computed once before
execution. A further analysis of this is provided in Section VI.

The cost function (14a) consists of a set of errors
$\mathbf{\widetilde{y}}_{k}$ dependent on the states and the inputs of the
system, and weighted by a diagonal matrix $\mathbf{Q}_{k}$ for every shooting
node. Different values of $\mathbf{\widetilde{y}}_{k}$ are used to model the
terminal and running costs (reference frames omitted for brevity):

$\mathbf{\widetilde{y}}_{k}=\begin{cases}\left[{\left[\boldsymbol{\gamma}_{k}\frac{T}{N}+\mathbf{u}_{k}\frac{T^{2}}{2N^{2}}\right]}^{T}\,\mathbf{{w}}^{T}_{k}\,{\widetilde{r}\left(\textbf{x}_{k}\right)}\right]^{T}&k\in\left[0,N-1\right]\\\
\left[\mathbf{\widetilde{p}}^{T}_{k}\,\mathbf{\widetilde{q}}^{T}_{k}\,\mathbf{\widetilde{v}}^{T}_{k}\,\mathbf{\widetilde{w}}^{T}_{k}\right]^{T}&k=N\end{cases}$
(15)

It can be seen that $\mathbf{\widetilde{y}}_{k}\in\mathbb{R}^{8}$ for the
running cost and $\mathbf{\widetilde{y}}_{k}\in\mathbb{R}^{12}$ for the
terminal cost. The running cost minimizes the integral of the motor thrusts
(which is
$\int_{0}^{\frac{T}{N}}\left(\boldsymbol{\gamma}+\boldsymbol{u}t\right)d_{t}=\boldsymbol{\gamma}\frac{T}{N}+\mathbf{u}\frac{T^{2}}{2N^{2}}$
) , as well as the angular velocities $\mathbf{{w}}_{k}$ of the robot and the
reprojection error $\widetilde{r}_{k}$ of the objective line. The terminal
cost minimizes the position and orientation error
$\mathbf{\widetilde{p}}_{k},\mathbf{\widetilde{q}}_{k}$ as well as the final
linear and angular velocity errors
$\mathbf{\widetilde{v}}_{k},\mathbf{\widetilde{w}}_{k}$ at the desired
perching state $\mathbf{x}_{perch}$. The constraints (14h)-(14j) are always
present during the whole horizon prediction. However, while satisfying
constraint (14j) is critical to avoid collisions with powerlines, doing so for
the perception constraints is not practical, since the camera may be mounted
at a different place from the perching mechanism, and thus will not see the
powerline at the final part of the maneuver. To solve this, we model the
constraints (14h),(14i) as soft constraints with exponentially decaying costs,
such that they are negligible at the end of the trajectory. We do the same for
the cost of $\widetilde{r}_{k}$. Finally, notice that since the lines are in
global axes (i.e., mapped with any state estimator that tracks their position
in $W$), the NLP does not need them to be inside the camera’s FOV in order to
work.

Figure 3: Developed procedure for perching+recovery trajectory integration.

###  V-B Trajectory and Recovery Integration

We are interested in perching trajectories that can recover the robot to a
safe position without any collisions even if the perching mechanism fails. To
do so, we use the same optimization problem from (14) with different cost
values (and without perception costs and constraints) to generate a recovery
trajectory that starts right after the perching trajectory finishes (see Fig.
3): First, we solve (14) to compute the perching trajectory. Second, we
integrate its result with a finer resolution (we used 1ms for the proposed
experiments) using a Runge-Kutta4 integration scheme. Third, we check the
integrated trajectory for any possible collisions between shooting nodes that
could have not been detected when solving the NLP. If necessary, we can solve
(14) again with a higher $N$ using the current solution as a warm start (we
found $N=30$ is usually good enough for trajectories of several meters and
$T\leq 5$ s). Finally, we use the end of the perching trajectory as the
beginning of the recovery and solve (14) for it, also integrating its result
afterwards and adding it after the integrated perching maneuver.

The resulting trajectory is continuous for the whole maneuver, intrinsically
leads to a safe recovery if the perching is not completed, and is dynamically
feasible.

Figure 4: Actual robot trajectory performed by the resulting NMPC in an
experiment where the provided reference had two collisions (red zones). The
front of the robot is colored in green. The powerlines appear in red. The
numbers denote the commanded waypoints.

##  VI Experimental Validation

The proposed method was evaluated onboard a custom quadrotor platform
developed at the Robotics and Perception Group (RPG) of the University of
Zurich, with a weight of 0.8 kg and a thrust-to-weight ratio of 4:1. A Radix
FC board was used as the low level flight controller, and a NVIDIA Jetson TX2
as the main onboard computer. The robot was equipped with a Realsense D435i
camera in its front face. The state estimates of the quadrotor are given by an
optical tracking system running at 120Hz, while the positions of the mockup
powerlines are obtained by [20], a visual-inertial state estimator which maps
point and line features with 10Hz visual feedback (enough for mapping static
lines), running in the CPU of the system. It was adapted to map red lines by
only using the images’ red channel in its line search module.

The developed method was implemented in C++ following the scheme in Fig. 3,
and then interfaced as a ROS node. Different experiments were performed with
the developed NLP with and without perception awareness. To remove the
perception awareness, it is only necessary to disable constraints (14h) and
(14i), and remove (9) from the NLP, which is done by setting their
correspondent weights to zero. The control actuations of the quadrotor during
flight are computed by a NMPC controller from RPG described at [16]. The
controller runs at 100Hz using a Real-Time Iteration (RTI) scheme [23] and is
then cascaded with a high-frequency L1 adaptive controller [24] that corrects
for disturbances such as aerodynamic drag or model inaccuracies.

Figure 5: Collision avoidance constraint (14j), line reprojection error (9),
and line cheirality (14h) and segment visibility (14i) constraints during the
experiment in Fig. 4. The constraint violation zone is marked in red.

Finally, in this work we are not interested in the development of a specific
perching mechanism, but rather in providing the optimal positioning that such
mechanisms would require for the perching to happen. Thus, we focus the
experiments on the maneuvering itself and always recover the quadrotor to a
safe position after reaching the perching state.

###  VI-A Inspection experiments

First, we validate the mathematical modeling from Section IV decoupled from
the perching trajectory generation system. To do so, we incorporate
constraints (14h)-(14j) and cost (9) into the onboard NMPC controller
described in [16], without making use yet of the proposed NLP trajectory
generation system. The resulting controller is validated through missions
where the robot performs powerline inspection (see Fig. 1 for a visual clue of
the line setup). The robot is commanded to follow straight lines between a set
of waypoints which is intentionally thought to lead the robot to collide with
the powerlines and to maintain them far from the center of the camera. The
resulting NMPC controller follows the given trajectories adapting its yaw and
height to avoid collisions while maximizing the visibility of the required
powerline. Fig. 4 shows that the trajectory actually performed by the robot
successfully avoided the two potential collisions in contrast to the waypoint
trajectory. Fig. 5 compares the reprojection error between the commanded
trajectory and the one actually followed by the NMPC controller, showing a
mean improvement of 500%, which involves 100 pixels. The perception and
collision avoidance constraints are satisfied during the whole flight, except
for the segment visibility constraint during brief instants. This exhibits the
advantages of including the proposed modeling in the NMPC controller. Another
advantage is that the line positions can be updated online since the NMPC
executes at real time. However, notice that the controller still requires a
feasible trajectory or reference to follow. For agile perching, this can not
be simply the desired pose or a hover-to-hover minimum snap trajectory. The
proposed NLP solves the trajectory generation and is validated in Section
VI-B. Moreover, since the NLP already accounts for collision avoidance and
perception awareness, the controller will not need to include these, so other
trajectory-tracking controllers with lower computational demand could be used
[16].

More than 20 experiments were performed providing the robot with various
trajectories and powerline configurations, and the resulting NMPC always
achieved similar results. All these experiments were performed with up to
three segments and with a 100 Hz RTI control rate with the only requirement of
formulating all the included constraints as soft ones, whose costs are started
at zero and are slowly increased at the beginning of the flights. If more than
one line should be inspected at the same time, one could append more costs and
constraints for each line (increasing the computational cost), or track the
centroid of the lines as an intermediate solution.

###  VI-B Perching experiments

Figure 6: Comparison of two perching maneuvers with and without perception
awareness. The perception-aware maneuver (orange) is followed by the black
quadrotor, while the other (blue) is followed by the grey quadrotor. The green
lines show the orientation of the robot’s camera at each instant.

We now validate the whole proposed perching trajectory generation system in
different maneuvers that are computed and executed onboard the robot. We
assume the perching end-effector is installed at the bottom of the quadrotor
as in [11]. We add an additional degree of freedom to the end-effector, and
assume its yaw orientation can be controlled, so we can better illustrate the
effect of perception awareness.

Three powerlines were set up with different inclinations (see Fig. 1). The
robot was set to hover in front of them with its camera parallel to the lines.
The robot first performs a perching maneuver to reach the closest line at 80°
without including perception awareness. The robot is able to follow the
trajectory, reach the perching pose with zero velocity, and recover to a safe
position afterwards. Then, the maneuver is computed and executed for the same
end pose, but including perception awareness. Fig. 6 shows both trajectories
from the same initial point. By performing an initial correction, the
quadrotor is able to reach the same end pose while keeping the powerline
centered in its camera during most of the trajectory. Fig. 7 shows the
evolution of the reprojection error and the segment visibility constraint
during the maneuver, which was more favorable in the perception-aware case. A
comparison of final position and orientation errors within multiple flights is
given later on Table I.

Figure 7:  Line reprojection error (9) and segment visibility constraint (14i)
during both maneuvers in Fig. 6. The obstacle avoidance (14j) and line
cheirality (14h) constraints are always satisfied for both maneuvers and thus
are not shown. The constraint violation zone is marked in red.

After showing the capabilities for perception-awareness, we test its
functioning in extreme perching maneuvers, where the drone stops completely
upside down at the perching pose. We note that this has currently only been
done with quadrotors with bi-directional thrust capabilities [11]. We set up
the bottom powerline at a height of 3’7 m, leaving roughly 2’5 m of operation
for the quadrotor in the Z axis (its minimum allowed height is of 0.8 m). The
robot is set to hover in front of the line and then computes and executes the
required perching maneuver. Its tracking can be seen in Fig. 8. Moreover, the
posterior recovery of the system is also accomplished without reaching the
demanded minimum height. In this case the effect of the perception awareness
becomes negligible unless its corresponding costs are significantly increased,
resulting in a divided maneuver that first moves towards the line while
keeping it in view and then only performs perching when the perception costs
decay. This is an expected outcome since the already restrictive demanded
maneuver leaves no margin to reorientate the robot’s front during it. Thus,
this result is not further analyzed.

Maneuver |  $\mathbf{{p}}_{RMSE}$ (cm) |  $\mathbf{{q}}_{RMSE}$ (°) |  $t_{p}$ (ms) |  $t_{r}$ (ms)
---|---|---|---|---
80 ° | 3.1 | 2.0 | 432 | 221
80 °+ PA | 3.4 | 2.6 | 1526 | 584
20 ° | 2.7 | 2.1 | 457 | 351
20 °+ PA | 4.8 | 7.2 | 1893 | 461
180 ° | 6.9 | 3.0 | 532 | 1214
TABLE I: Position and pitch-roll rmse of different tracked perching
trajectories, and computational time to produce them.

Finally, we analyze the average performance of the whole system in a set of
different perching experiments. We repeat the experiments presented in Figs. 6
and 8, and add an additional experiment where the robot performs a perching
maneuver to the top line in Fig. 6, at a pitch angle of 20°, with and without
perception awareness. For 5 experiments of each maneuver, we compute the mean
RMSE position and orientation errors at the perching point, as well as the
time required for computing the perching and recovery maneuvers. Table I
summarizes these results. We observed that in general the trajectories could
be tracked with fairly low position error with the exception of the upside-
down perching. This could potentially be improved with a finer tuning of the
onboard controller, though it can be compensated by having a certain degree of
tolerance in the perching mechanism. On the other hand, we found that the
performance of perception-aware maneuvers was highly dependent on the starting
position with respect to the objective line (all performed maneuvers started
at the same relative position to the whole setup). For example, the 20°PA
maneuver was harder to compute and follow since it was started from a lower
altitude than the objective line, while this was not the case in the one with
80°. This opens future research on how to compute the optimal starting point
for a perching maneuver.

Figure 8: Tracked perching maneuver where the robot finishes upside-down. The
numbers denote the evolution of the trajectory.

##  VII Conclusions and Future Work

In this work we presented a novel perching trajectory generation framework
which generates highly versatile perching trajectories that satisfy collision
avoidance with powerlines and maximize their visibility during flight. The
efficacy of our method was demonstrated on a set of real world experiments
onboard a computationally limited quadrotor. We show that the quadrotor is
capable of executing the perching trajectory with minimal tracking error and
complete obstacle avoidance, even at very high angles of attack. If the
perching mechanism were to fail, our algorithm provides a fail safe trajectory
such that the drone automatically recovers and maintains flight. Additionally,
we show that our formulation is capable of running inside an onboard
controller in real time, providing it with capabilities for inspection of
powerlines, avoiding collisions with them and ensuring that the inspected line
is kept in view at all times. In the future, we want to explore how the
starting point of a perching trajectory impacts its performance, and how to
utilize this information to increase the likelihood of a successful perching.
We also hope to explore how multiple cameras or sensors can be taken into
account simultaneously into the same perching maneuver, combining their
potential at the parts of the maneuver where they best suit for.

## Acknowledgments

The authors would like to thank Thomas Laengle for his help in the setup of
the quadrotor used for the experiments, Philipp Foehn and Elia Kaufmann for
their helpful insights on the first version of the manuscript, and Leonard
Bauersfeld and Víctor Valseca for their help in the preparation of the media
material.

## References

  * [1] S. Hoff, Jul 2016. [Online]. Available: https://www.eia.gov/todayinenergy/detail.php?id=27152
  * [2] R. Kitchen, N. Bierwolf, S. Harbertson, B. Platt, D. Owen, K. Griessmann, and M. A. Minor, “Design and evaluation of a perching hexacopter drone for energy harvesting from power lines,” in _IEEE/RSJ IROS_ , 2020, pp. 1192–1198.
  * [3] D. Mellinger, N. Michael, and V. Kumar, “Trajectory generation and control for precise aggressive maneuvers with quadrotors,” _The Int. Journal of Robotics Research_ , vol. 31, no. 5, pp. 664–674, 2012.
  * [4] J. Thomas, M. Pope, G. Loianno, E. W. Hawkes, M. A. Estrada, H. Jiang, M. R. Cutkosky, and V. Kumar, “Aggressive Flight With Quadrotors for Perching on Inclined Surfaces,” _Journal of Mechanisms and Robotics_ , vol. 8, no. 5, 05 2016.
  * [5] K. Hang, X. Lyu, H. Song, J. A. Stork, A. M. Dollar, D. Kragic, and F. Zhang, “Perching and resting—a paradigm for uav maneuvering with modularized landing gears,” _Science Robotics_ , vol. 4, 2019.
  * [6] J. Thomas, G. Loianno, K. Daniilidis, and V. Kumar, “Visual servoing of quadrotors for perching by hanging from cylindrical objects,” _IEEE robotics and automation letters_ , vol. 1, no. 1, pp. 57–64, 2015.
  * [7] L. Daler, A. Klaptocz, A. Briod, M. Sitti, and D. Floreano, “A perching mechanism for flying robots using a fibre-based adhesive,” in _IEEE ICRA_ , 2013, pp. 4433–4438.
  * [8] H. Tsukagoshi, M. Watanabe, T. Hamada, D. Ashlih, and R. Iizuka, “Aerial manipulator with perching and door-opening capability,” in _IEEE ICRA_ , 2015, pp. 4663–4668.
  * [9] J. Mao, G. Li, S. Nogar, C. Kroninger, and G. Loianno, “Aggressive visual perching with quadrotors on inclined surfaces,” _arXiv preprint, arXiv ID:2107.11171_ , 2021.
  * [10] K. M. Popek, M. S. Johannes, K. C. Wolfe, R. A. Hegeman, J. M. Hatch, J. L. Moore, K. D. Katyal, B. Y. Yeh, and R. J. Bamberger, “Autonomous grasping robotic aerial system for perching (agrasp),” in _IEEE/RSJ IROS_ , 2018, pp. 1–9.
  * [11] P. Yu, G. Chamitoff, and K. Wong, “Perching upside down with bi-directional thrust quadrotor,” in _2020 International Conference on Unmanned Aircraft Systems (ICUAS)_ , 2020, pp. 1697–1703.
  * [12] D. Falanga, P. Foehn, P. Lu, and D. Scaramuzza, “Pampc: Perception-aware model predictive control for quadrotors,” in _IEEE/RSJ IROS_ , 2018, pp. 1–8.
  * [13] M. Jacquet, G. Corsini, D. Bicego, and A. Franchi, “Perception-constrained and motor-level nonlinear mpc for both underactuated and tilted-propeller uavs,” in _IEEE ICRA_ , 2020, pp. 4301–4306.
  * [14] M. Greeff, T. D. Barfoot, and A. P. Schoellig, “A perception-aware flatness-based model predictive controller for fast vision-based multirotor flight,” _IFAC-PapersOnLine_ , vol. 53, no. 2, pp. 9412–9419, 2020.
  * [15] G. Phillips, “Algorithms for piecewise straight line approximations,” _The Computer Journal_ , vol. 11, no. 2, pp. 211–212, 1968.
  * [16] S. Sun, A. Romero, P. Foehn, E. Kaufmann, and D. Scaramuzza, “A comparative study of nonlinear mpc and differential-flatness-based control for quadrotor agile flight,” _arXiv preprint: 2109.01365_ , 2021.
  * [17] M. Faessler, D. Falanga, and D. Scaramuzza, “Thrust mixing, saturation, and body-rate control for accurate aggressive quadrotor flight,” _IEEE Robot. Autom. Lett._ , vol. 2, no. 2, pp. 476–482, Apr. 2017.
  * [18] B. Houska, H. Ferreau, and M. Diehl, “ACADO Toolkit – An Open Source Framework for Automatic Control and Dynamic Optimization,” _Optimal Control App. and Methods_ , vol. 32, no. 3, pp. 298–312, 2011.
  * [19] J. A. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “Casadi: a software framework for nonlinear optimization and optimal control,” _Math. Programming Comp._ , vol. 11, no. 1, pp. 1–36, 2019.
  * [20] Q. Fu, J. Wang, H. Yu, I. Ali, F. Guo, Y. He, and H. Zhang, “Pl-vins: Real-time monocular visual-inertial slam with point and line features,” _arXiv preprint: 2009.07462_ , 2020.
  * [21] A. Domahidi and J. Jerez, “Forces professional,” Embotech AG, url=https://embotech.com/FORCES-Pro, 2014–2021.
  * [22] A. Zanelli, A. Domahidi, J. Jerez, and M. Morari, “Forces nlp: an efficient implementation of interior-point methods for multistage nonlinear nonconvex programs,” _Int. Journal of Control_ , 2017.
  * [23] M. Diehl, H. G. Bock, H. Diedam, and P.-B. Wieber, “Fast direct multiple shooting algorithms for optimal robot control,” in _Fast motions in biomechanics and robotics_. Springer, 2006, pp. 65–93.
  * [24] D. Hanover, P. Foehn, S. Sun, E. Kaufmann, and D. Scaramuzza, “Performance, precision, and payloads: Adaptive nonlinear mpc for quadrotors,” _IEEE Robotics and Automation Letters_ , 2021.