11institutetext:  University of Potsdam, Institute of Computer Science,
Potsdam, Germany
11email<EMAIL_ADDRESS>orcid:0000-0001-8888-7475

# The Coq Proof Script Visualiser (coq-psv)††thanks: Special thanks to
Sebastian Böhne who gave the idea and concept to the tool. 111This work was
presented during a talk at the Coq Workshop 2020

Mario Frank

The Coq proof assistant [2] is used in various scientific, industrial, and
teaching contexts and enables scientists, teachers, and students to exchange
formalisations in an easy way. But this exchange is limited to the vernacular
files and the respective LaTeX representation generated by coqdoc [3], for
example. Also, those representations merely contain the definitions and proof
scripts but not the information about the specific proof steps, i.e. the
current goals and hypotheses. Thus, in order to fully understand the specific
proofs, including the way how hypotheses change, the recipient needs to
acquire a compatible version of Coq. This can be cumbersome as the
installation of the specific Coq version and necessary additional tools is not
always straight-forward due to operating system and Opam version differences.
To our knowledge, there currently is no tool that is able to represent Coq
proofs on paper including all used tactics, goals and hypotheses in a readable
and printable manner as tree representations usually do not scale well for
long proofs.

## Applications.

The Coq Proof Script Visualiser aims at supporting the development, teaching,
and review process of proofs by exporting the proof scripts together with the
current proof state for each proof step. As all information about the proof is
then at hand, readers merely need to know the semantics of the used tactics to
understand the proof. Proof developers can benefit from additional warnings
given by coq-psv when superfluous structuring is used. Reviewers can retrace
the proof more easily without the need to install Coq. In teaching, the
generated output can be easily modified to be used as a cloze where students
have to fill in the matching tactic or introduced hypothesis, for example. And
most notably, the exported proof representations can be used for archiving
purposes and enriching the output of coqdoc.

## Approach.

Given a Coq file, coq-psv processes it using the Coq parsing functionality and
data structures. Although the output of coq-psv is linear, it internally
generates a labelled proof tree for each proof in the file as using a tree
structure simplifies the analysis and augmenting of the proof information. The
label itself contains the name and type of the proven statement (e.g. theorem
or lemma) together with the statement itself and the information, how the
proof was completed (Qed or Admitted). The tree consists of nodes that contain
the used tactic or command, and the resulting hypotheses and goals. While the
extraction of the label content is easy, building the tree nodes is much more
complex. This is due to the numerous ways in Coq to branch in a proof
situation, which includes bullets, brackets and the (deprecated) Focus
command. After extracting the labelled trees, they are translated to LaTeX
tables according to the given command line options. We will give insight into
the way we handle different branching options and why we do so in the talk.

Figure 1: Excerpt of a proof visualisation

## Versatile Use.

The coq-psv supports many command line options that define the output
behaviour. It is able to process both single Coq files and complete Coq
projects. Furthermore, it is possible to generate either complete LaTeX
documents or only the LaTeX tables. Also, all proofs from a vernacular file
can be exported either to one single file or to separate files per proof. The
latter simplifies the integration into other LaTeX documents. There are two
different table layouts: The first one mimics the goal and hypothesis
visualisation of CoqIDE while the second one represents the goals and
hypotheses in sequent style. Also, it is possible to hide goals and hypotheses
in the step in which they are created if all of them are handled by bullets.
This can reduce the size of the output significantly. Hiding hypotheses that
remain unchanged until the end of the (sub-)proof and marking them as
invariant on introduction is also supported, which again condenses the size of
the tables (see Figure 1). Finally, the layout of the tables can be modified
after the generation. This can be done by overriding the default LaTeX
commands used in the generated files before importing them. For example the
size of the table itself and of specific table columns can be set to a desired
value without having to modify the table itself.

## Related work.

There are already some approaches to representing proof scripts in a more
readable and independent way (see [5, 7, 8, 9, 11, 10], for example). But
except for [5] and [11], all of them represent the proofs only as trees or
diagrams. While trees and diagrams can give a good overview about the
structure of a proof, printing them in a readable way can be problematic for
long proofs. Although the approach described in [5] supports both a tree and a
Fitch style representation, the latter only shows the current step and it does
not seem to be possible to export the complete proof as LaTeX file, for
example. Also, only ProofWeb [5], Proviola [11], ProofTree [10] and Traf [7]
are targeting Coq and the two latter only work in a live session with Coq and,
for example, ProofGeneral [1]. Furthermore, Traf uses ProofTree and both seem
to have been discontinued for more than two years. according to the project
pages [10, 12]. Proviola is able to generate an animated version of a proof
script giving good insight into the proving process, but it does not focus on
printing the output on paper. Anther project makes Coq proofs available in the
browser (see [6]) which makes the local installation of Coq unnecessary and
improves usability as documents can be shared. But it does not seem to be
possible to generate a printable version with all necessary information as
coq-psv does.

## Current state and future work.

The tool should be currently seen as proof of concept and supports Coq 8.10.
It is implemented in OCaml and can be installed from our Opam repository as
described on the coq-psv project page [4]. There are currently some minor
issues in the vertical alignment of the used tactic. But even non-trivial
proofs can be visualised decently as can be seen in some examples on the
project page. It is possible to generate LaTeX and PDF files. Obviously, there
are many options for future work. For example, one could hide tactics like
clear, rename and move and give them a special treatment instead: for
instance, marking cleared hypotheses, annotating identifier changes with the
substitution pattern and just change the position of the displayed hypotheses.
Then, a support for modules in files should be incorporated. Furthermore,
supporting especially HTML output is a main aspect of future work. We aim at
integrating such an improved coq-psv into coqdoc. Additionally, coq-psv could
be extended to improve the proof scripts themselves. For instance, structure
improvements can be suggested. Especially, unused or irrelevant hypotheses
might be detected and automatically deleted, if desired. This is not an easy
task since it may change the names of the hypotheses appearing later on.
Finally, one could give the option to conduct an automatic reordering of the
lines in a proof script.

## References

  * [1] David Aspinall.  Proof General: A Generic Tool for Proof Development.  In Susanne Graf and Michael Schwartzbach, editors, Tools and Algorithms for the Construction and Analysis of Systems, pages 38–43. Springer, 2000.
  * [2] The Coq proof assistant.  http://coq.inria.fr.  (Last visited 18.06.2020).
  * [3] The coqdoc wiki page.  https://github.com/coq/coq/wiki/Coqdoc.  (Last visited 18.06.2020).
  * [4] Coq Proof Script Visualiser project page.  https://www.cs.uni-potsdam.de/coq-psv.  (Last visited 18.06.2020).
  * [5] Maxim Hendriks, Cezary Kaliszyk, Femke Van Raamsdonk, and Freek Wiedijk.  Teaching Logic Using a State-of-the-Art Proof Assistant.  Acta Didactica Napocensia, 3(2):35–48, 2010.
  * [6] The JSCoq project page.  https://jscoq.github.io/.  (Last visited 18.06.2020).
  * [7] Hideyuki Kawabata, Yuta Tanaka, Mai Kimura, and Tetsuo Hironaka.  Traf: A Graphical Proof Tree Viewer Cooperating with Coq Through Proof General.  In Sukyoung Ryu, editor, Programming Languages and Systems, pages 157–165, Cham, 2018. Springer International Publishing.
  * [8] Tomer Libal, Martin Riener, and Mikheil Rukhaia.  Advanced Proof Viewing in ProofTool.  In Christoph Benzmüller and Bruno Woltzenlogel Paleo, editors, Proceedings Eleventh Workshop on User Interfaces for Theorem Provers (UITP 2014), volume 167 of EPTCS, pages 35–47, 2014.
  * [9] Jorge Pais and Álvaro Tasistro.  Proof Assistant Based on Didactic Considerations.  Journal of Universal Computer Science, 19(11):1570–1596, 2013.
  * [10] The ProofTree project page.  https://askra.de/software/prooftree/.  (Last visited 18.06.2020).
  * [11] Carst Tankink, Herman Geuvers, James McKinna, and Freek Wiedijk.  Proviola: A tool for proof re-animation.  CoRR, abs/1005.2672, 2010.
  * [12] The Traf project page.  https://github.com/hide-kawabata/traf.  (Last visited 18.06.2020).


# Optimization-based Trajectory Tracking Approach for Multi-rotor Aerial
Vehicles in Unknown Environments

Geesara Kulathunga1, Hany Hamed2, Dmitry Devitt2, Alexandr Klimchik2
Manuscript received: September, 9, 2021; Revised December, 3, 2021; Accepted
January, 29, 2022.This paper was recommended for publication by Editor Stephen
J. Guy upon evaluation of the Associate Editor and Reviewers’ comments. This
work was supported by The Analytical Center for the Government of the Russian
Federation (Agreement No. 70-2021-00143 dd. 01.11.2021, IGK
000000D730321P5Q0002).1Artificial Intelligence Research Center, Innopolis
University, Russia<EMAIL_ADDRESS>Centre for Robotics and Mechatronics
Components, Innopolis University, Russia<EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS>Object Identifier (DOI):
see top of this page.

###### Abstract

The goal of this paper is to develop a continuous optimization-based
refinement of the reference trajectory to ’push it out’ of the obstacle-
occupied space in the global phase for Multi-rotor Aerial Vehicles in unknown
environments. Our proposed approach comprises two planners: a global planner
and a local planner. The global planner refines the initial reference
trajectory when the trajectory goes either through an obstacle or near an
obstacle and lets the local planner calculate a near-optimal control policy.
The global planner comprises two convex programming approaches: the first one
helps to refine the reference trajectory, and the second one helps to recover
the reference trajectory if the first approach fails to refine. The global
planner mainly focuses on real-time performance and obstacles avoidance,
whereas the proposed formulation of the constrained nonlinear model predictive
control-based local planner ensures safety, dynamic feasibility, and the
reference trajectory tracking accuracy for low-speed maneuvers, provided that
local and global planners have mean computation times 0.06s (15Hz) and 0.05s
(20Hz), respectively, on an NVIDIA Jetson Xavier NX computer. The results of
our experiment confirmed that, in cluttered environments, the proposed
approach outperformed three other approaches: sampling-based pathfinding
followed by trajectory generation, a local planner, and graph-based
pathfinding followed by trajectory generation.

###### Index Terms:

Constrained Motion Planning, Planning under Uncertainty, Collision Avoidance.

##  I INTRODUCTION

The reference trajectory tracking for multi-rotor aerial vehicles (MAVs) is
used in various domains, e.g., cinematography, or landing on a moving
platform. Even though many approaches have been proposed for tracking
specified reference trajectories [1, 2, 3], it remains an open research
problem due to several reasons: achieving real-time performance, avoiding
close-in obstacles, adhering to different weather conditions, etc.
Subsequently, generating a near-optimal control policy for maneuvering through
a cluttered unknown environment is a rather challenging task when enforcing
the dynamic feasibility and safety constraints in real-time. Model Predictive
Control (MPC) is one of the promising techniques to address such challenging
tasks. However, due to the computational aspects of MPC, it is difficult to
achieve real-time performance in most situations using limited available
resources [4]. Such aspects are mainly because of the way the problem is
formulated, e.g., as an NMPC (Nonlinear MPC), as an LMPC (Linear MPC), and the
way constrains, e.g., obstacles and inputs, are handled. Moreover, the
accuracy of the near-optimal policy generation of MPC depends on the sensing
capabilities, e.g., FoV (Field of View), sensing distance, and the way
surrounded free space and obstacles are represented. For instance, free space
can be formed as a set of convex polyhedrons along the refined reference
trajectory. Afterwards, incorporating a linear motion model, the problem can
be formulated as convex rather than non-convex.

Figure 1: Experiment with the proposed trajectory tracker to showcase how the
proposed approach works in real conditions. In this experiment, the total
distance of R was about 54m, and the length of $P_{t_{n}}$ was set to 10

This paper proposes an optimization-based approach that solves two
trajectories simultaneously, when the first one tries to refine the initial
reference trajectory pushing the reference trajectory away from the known
obstacles, while the second one generates a near-optimal control policy at
every planning step incorporating the refined trajectory (Fig.1). Thus, the
contributions of this work are as follows:

  1. 1.

Developing a framework for reference trajectory tracking, ensuing safety and
dynamic feasibility in which the global planner refines the reference
trajectory allowing the local planner to generate a near-optimal control
policy quickly at every planning iteration

  2. 2.

Proposing a fast approach, formulated as a convex problem, for pushing the
reference trajectory away from obstacle zones, where we implemented a parallel
version of Convex Decomposition [5] (Algorithm 1, line 9) and a simplified
approach, as compared to conservative approaches in prior work, for time
allocation

  3. 3.

Real-world and simulated experiments that showcase the agile flights in
various unknown cluttered environments and a new dataset we used for
benchmarking our approach with the three other approaches

##  II Related Work

Figure 2: The high-level system architecture of the proposed trajectory
tracker. The global and local planners run in parallel as two separate threads
while sharing the reference trajectory. Each of the components, i.e., Global,
Local, Mapper, average frequencies, are estimated on an NVIDIA Jetson Xavier
NX computer (see Fig. 10), which is utilized for real-world experiments

Most of the recent trajectory tracking approaches are formulated as
optimization problems where all the constraints are incorporated in close loop
manner. Moreover, such constraints can be embodied as a part of the objective
or as a part of the constraints, which can be either soft or hard constraints
[6, 7, 8]. Such problem formulation, i.e., employing both objective and
constraints, can belong to one of the types: convex [9] or non-convex [4]
(non-linear). Non-convex problem formulation is usually computationally
expensive. In the recent work of Liu et al. [10], a successive convex
decomposition-based free-space representation, i.e., a series of overlapping
polyhedra, was proposed. Such a free-space representation helps to keep MAV
within the free space for the given interval. Specific interval allocation can
be calculated in several different ways: let the solver allocate intervals
[11] or define several intervals prior to solving. Once intervals are
allocated, different methods can be used for time allocation (fixed or
adaptive) [5].

Trajectory tracking problem can be solved during two different planning
stages: local or/and global, in which local and global planners can be
formulated as two separate or one combined optimization problem. As far as
MAVs are concerned, most of the approaches exploit differential flatness
property, where the smoothness and dynamic feasibility are estimated by
minimizing the L2 norm of velocity difference over the trajectory [12].
However, problem formulation can be complex when enforcing different
constraints [13], e.g., obstacle, time, and input constraints. Thus, in the
literature, various approaches have been proposed to handle the said
constraints. The most primitive paradigm is to use path planning followed by
trajectory generation [14, 15], which can be considered as an open-loop
problem. However, such approaches fail due to the high computation time, as
well as when the environment is highly dynamic and cluttered. To reduce the
computation time and have fast reaction time, motion primitive-based local
trajectory planning [16, 17, 18] were proposed. Such approaches are often
trapped in local minima. Hence, the objectives of global planer and local
planner can differ mainly due to the expected nature (or characteristics) of
problem formulation as follows:

The functions and characteristics of a local planner are: it plans for a local
near-optimal trajectory based on the currently perceived information within
the close vicinity with higher accuracy [3, 19]; it generates near-optimal
control policy in an online fashion in every iteration; it performs trajectory
smoothing and feasibility checking to ensure the differential/dynamic
constraints; the trajectories are planned consecutively, depending on the way
they handle the next set of information that comes in, e.g., how they react to
dynamic and static obstacles and how they decide whether to incorporate
previous information[1, 2]; long horizon-based trajectory planning may
generate wasteful unnecessary long trajectories [4].

The functions and characteristics of a global planner are: it tries to plan
the global near-optimum trajectories [20]; it constructs the map of the way,
either memory-less or fusion-based [17], (a memory-less map does not consider
any previous information, but rather considers only current map; a fusion-
based map building does not discard stale data, i.e., previous information,
which might be problematic for dynamic obstacles, i.e., some free-known space
could be considered as occupied-known space [14]); with the known mapping,
global planner generates the obstacle-free kinematically feasible trajectory
that often ensures the differential/dynamic constraints; it minimizes
backtracking and generates efficient trajectories in cluttered environments
[17] in which maintaining a clear picture of the environment is a heavy burden
on computational perspective; the computational complexity depends on how much
information is incorporated and the way a problem is formulated, e.g.,
constrained/unconstrained, linear/non-linear optimization problem [8, 9, 20].

Therefore, safety, appropriate maneuver, dynamic feasibility, and real-time
performance are the main objectives that trajectory tracker must have. The
safety and feasible trajectory generation of local or global planners depend
on the manner free-known and free-unknown spaces are incorporated. Hence,
planning space can be defined to lie within the sensor’ FOV or outside of it
[21, 22, 23], provided that a series of sensor data has been incorporated for
constructing the environment map. When the environment is cluttered, local
planners perform poorly due to the uncertainty of instance sensing data.
Prediction horizon-based planning is commonly used for such environments, in
which local planners can be more conservative compared to global planners.
Such a local planner can be formed in multiple ways, e.g., Linear Model
Predictive Control (MPC) [24], Nonlinear Model Predictive Control (NMPC) [4] ,
and Corridor-based Model Predictive Contouring Control(CMPCC) [25], based on
the necessity and the requirements. The global planner must be less
conservative compared to the local planner, e.g., when defined as an
unconstrained function minimizer. In the proposed approach, the local planner
is formed as an NMPC, whereas the global planner is formed as a box-
constrained function minimizer.

Accurate environment mapping is important to perform robust planning. Out of
many, memory-less and fusion-based are the main methods that are used for
mapping the environment [9]. Memory-less methods rely on the instantaneous
data, i.e., most of the time only on the last sensor reading, whereas fusion-
based methods - on the stacking sensor readings in a specific form, e.g.,
Octomap [26], Voxblox [27], as a map. Such methods may have considerable
estimation error and high computation time that depend on the hardware and
sensors capabilities. However, estimation error that emerged due to drift and
poor sensing measurement can be overcome by resetting the fusion from time to
time. Therefore, the latter methods are preferred over the memory-less methods
specially for reasoning of cluttered environments due to several reasons, such
as limited FOV and the lack of prior information about previous sensing data.
Once a map is constructed, Euclidean distance transform mapping (EDTM) [28]
can be utilized to estimate the free distance from a considered position. For
the mapping, we built instantaneous EDTM on top of Octomap.

##  III Methodology

The proposed approach uses a parallel architecture, which consists of a local
and a global planner, and where the global planner pushes the initial
reference trajectory away from obstacle zones $O_{m}$, whereas the local
planner generates an optimal control policy for tracking the modified
reference trajectory by global planner. The high-level view of the proposed
reference trajectory tracker is given in Algorithm 1. A pictorial
visualization of the notion that is used throughout the paper is shown in Fig.
3. The known obstacles and unknown obstacles are defined as
$O_{m}\in\leavevmode\hbox to12.1pt{\vbox
to12.1pt{\pgfpicture\makeatletter\hbox{\hskip 6.04839pt\lower-6.04839pt\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{
}\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{
}\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }{
{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{
}{{}{{{}}}{{}}{}{}{}{}{}{}{}{}{}{{}\pgfsys@moveto{5.84839pt}{0.0pt}\pgfsys@curveto{5.84839pt}{3.23001pt}{3.23001pt}{5.84839pt}{0.0pt}{5.84839pt}\pgfsys@curveto{-3.23001pt}{5.84839pt}{-5.84839pt}{3.23001pt}{-5.84839pt}{0.0pt}\pgfsys@curveto{-5.84839pt}{-3.23001pt}{-3.23001pt}{-5.84839pt}{0.0pt}{-5.84839pt}\pgfsys@curveto{3.23001pt}{-5.84839pt}{5.84839pt}{-3.23001pt}{5.84839pt}{0.0pt}\pgfsys@closepath\pgfsys@moveto{0.0pt}{0.0pt}\pgfsys@stroke\pgfsys@invoke{
} }{{{{}}\pgfsys@beginscope\pgfsys@invoke{
}\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-4.58334pt}{-3.41666pt}\pgfsys@invoke{
}\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{{M}}
}}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}}
\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} }
\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope{{{}}}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}}$ and
$O_{u}\not\in\leavevmode\hbox to12.1pt{\vbox
to12.1pt{\pgfpicture\makeatletter\hbox{\hskip 6.04839pt\lower-6.04839pt\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{
}\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{
}\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }{
{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{
}{{}{{{}}}{{}}{}{}{}{}{}{}{}{}{}{{}\pgfsys@moveto{5.84839pt}{0.0pt}\pgfsys@curveto{5.84839pt}{3.23001pt}{3.23001pt}{5.84839pt}{0.0pt}{5.84839pt}\pgfsys@curveto{-3.23001pt}{5.84839pt}{-5.84839pt}{3.23001pt}{-5.84839pt}{0.0pt}\pgfsys@curveto{-5.84839pt}{-3.23001pt}{-3.23001pt}{-5.84839pt}{0.0pt}{-5.84839pt}\pgfsys@curveto{3.23001pt}{-5.84839pt}{5.84839pt}{-3.23001pt}{5.84839pt}{0.0pt}\pgfsys@closepath\pgfsys@moveto{0.0pt}{0.0pt}\pgfsys@stroke\pgfsys@invoke{
} }{{{{}}\pgfsys@beginscope\pgfsys@invoke{
}\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-4.58334pt}{-3.41666pt}\pgfsys@invoke{
}\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{{M}}
}}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}}
\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} }
\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope{{{}}}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}}$, respectively.
Similarly, $F_{m}\in\leavevmode\hbox to12.1pt{\vbox
to12.1pt{\pgfpicture\makeatletter\hbox{\hskip 6.04839pt\lower-6.04839pt\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{
}\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{
}\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }{
{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{
}{{}{{{}}}{{}}{}{}{}{}{}{}{}{}{}{{}\pgfsys@moveto{5.84839pt}{0.0pt}\pgfsys@curveto{5.84839pt}{3.23001pt}{3.23001pt}{5.84839pt}{0.0pt}{5.84839pt}\pgfsys@curveto{-3.23001pt}{5.84839pt}{-5.84839pt}{3.23001pt}{-5.84839pt}{0.0pt}\pgfsys@curveto{-5.84839pt}{-3.23001pt}{-3.23001pt}{-5.84839pt}{0.0pt}{-5.84839pt}\pgfsys@curveto{3.23001pt}{-5.84839pt}{5.84839pt}{-3.23001pt}{5.84839pt}{0.0pt}\pgfsys@closepath\pgfsys@moveto{0.0pt}{0.0pt}\pgfsys@stroke\pgfsys@invoke{
} }{{{{}}\pgfsys@beginscope\pgfsys@invoke{
}\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-4.58334pt}{-3.41666pt}\pgfsys@invoke{
}\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{{M}}
}}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}}
\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} }
\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope{{{}}}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}}$ and
$F_{u}\not\in\leavevmode\hbox to12.1pt{\vbox
to12.1pt{\pgfpicture\makeatletter\hbox{\hskip 6.04839pt\lower-6.04839pt\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{
}\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{
}\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }{
{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{
}{{}{{{}}}{{}}{}{}{}{}{}{}{}{}{}{{}\pgfsys@moveto{5.84839pt}{0.0pt}\pgfsys@curveto{5.84839pt}{3.23001pt}{3.23001pt}{5.84839pt}{0.0pt}{5.84839pt}\pgfsys@curveto{-3.23001pt}{5.84839pt}{-5.84839pt}{3.23001pt}{-5.84839pt}{0.0pt}\pgfsys@curveto{-5.84839pt}{-3.23001pt}{-3.23001pt}{-5.84839pt}{0.0pt}{-5.84839pt}\pgfsys@curveto{3.23001pt}{-5.84839pt}{5.84839pt}{-3.23001pt}{5.84839pt}{0.0pt}\pgfsys@closepath\pgfsys@moveto{0.0pt}{0.0pt}\pgfsys@stroke\pgfsys@invoke{
} }{{{{}}\pgfsys@beginscope\pgfsys@invoke{
}\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-4.58334pt}{-3.41666pt}\pgfsys@invoke{
}\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{{M}}
}}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}}
\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} }
\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope{{{}}}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}}$ denote the known
free space and unknown free space. Hence, $F_{m}\cup
O_{m}\subseteq\leavevmode\hbox to12.1pt{\vbox
to12.1pt{\pgfpicture\makeatletter\hbox{\hskip 6.04839pt\lower-6.04839pt\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{
}\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{
}\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }{
{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{
}{{}{{{}}}{{}}{}{}{}{}{}{}{}{}{}{{}\pgfsys@moveto{5.84839pt}{0.0pt}\pgfsys@curveto{5.84839pt}{3.23001pt}{3.23001pt}{5.84839pt}{0.0pt}{5.84839pt}\pgfsys@curveto{-3.23001pt}{5.84839pt}{-5.84839pt}{3.23001pt}{-5.84839pt}{0.0pt}\pgfsys@curveto{-5.84839pt}{-3.23001pt}{-3.23001pt}{-5.84839pt}{0.0pt}{-5.84839pt}\pgfsys@curveto{3.23001pt}{-5.84839pt}{5.84839pt}{-3.23001pt}{5.84839pt}{0.0pt}\pgfsys@closepath\pgfsys@moveto{0.0pt}{0.0pt}\pgfsys@stroke\pgfsys@invoke{
} }{{{{}}\pgfsys@beginscope\pgfsys@invoke{
}\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-4.58334pt}{-3.41666pt}\pgfsys@invoke{
}\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{{M}}
}}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}}
\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} }
\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope{{{}}}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}}$ and all the unknown
region becomes $\mathbb{R}^{3}\backslash\leavevmode\hbox to12.1pt{\vbox
to12.1pt{\pgfpicture\makeatletter\hbox{\hskip 6.04839pt\lower-6.04839pt\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{
}\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{
}\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }{
{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{
}{{}{{{}}}{{}}{}{}{}{}{}{}{}{}{}{{}\pgfsys@moveto{5.84839pt}{0.0pt}\pgfsys@curveto{5.84839pt}{3.23001pt}{3.23001pt}{5.84839pt}{0.0pt}{5.84839pt}\pgfsys@curveto{-3.23001pt}{5.84839pt}{-5.84839pt}{3.23001pt}{-5.84839pt}{0.0pt}\pgfsys@curveto{-5.84839pt}{-3.23001pt}{-3.23001pt}{-5.84839pt}{0.0pt}{-5.84839pt}\pgfsys@curveto{3.23001pt}{-5.84839pt}{5.84839pt}{-3.23001pt}{5.84839pt}{0.0pt}\pgfsys@closepath\pgfsys@moveto{0.0pt}{0.0pt}\pgfsys@stroke\pgfsys@invoke{
} }{{{{}}\pgfsys@beginscope\pgfsys@invoke{
}\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-4.58334pt}{-3.41666pt}\pgfsys@invoke{
}\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{{M}}
}}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}}
\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} }
\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope{{{}}}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}}\subseteq(O_{u}\cup
F_{u})$.

Figure 3: Notion used for defining the reference trajectory tracker and
different spaces to the current pose $Q_{p}$ of MAV

Initial reference trajectory, namely $R$ consists of a set of control points:
$c_{i},i=0,...,N_{c}$, where the number of control points is given by $N_{c}$.
For generating $R$, we used the approach proposed in [4], which is based on
uniform bspline. The initialization time of the trajectory planning and
current time, where the desired pose on the reference trajectory should lie,
are denoted by $t_{0}$ and $t_{n}$, respectively. For a given time $t_{n}$,
starting and finishing control points are retrieved with respect to
$c_{s}\subseteq[0,N_{c})$ and $c_{e}\subseteq(c_{s},N_{c}-1]$ indices. The
time difference between two consecutive control points, i.e., $c_{i}$ and
$c_{i+1}$, is defined as $\delta_{t_{d}}$, which was set to 0.05s in our
experimental setup. The actual discretization time interval of continuous
system dynamics $\delta_{t_{c}}$ was set to 0.05. Besides, $\delta_{t_{c}}$
and $\delta_{t_{d}}$, which both can be the same or slightly different from
each other, can be configured. The number of control points within the
$R_{t_{n}}$, namely, refining horizon, is denoted by $N_{r}$. The avoidance
distance, which is the minimum free distance $D_{z}$ allowed in between MAV
and the closest obstacle to MAV.

Algorithm 1 Reference trajectory tracker

$\mathbf{Inputs}$: at time $t_{n}$,
${\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}}$:
reference trajectory to be refined, $Q_{p}$: current pose of MAV,
${\color[rgb]{0,0.19921875,0.3984375}\definecolor[named]{pgfstrokecolor}{rgb}{0,0.19921875,0.3984375}P_{t_{n}}}$:
trajectory to be tracked , $M_{t_{n}}$: EDT map of the environment

$\mathbf{Outputs}$:
${\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}}$:
refined reference trajectory;
${\color[rgb]{0,0.19921875,0.3984375}\definecolor[named]{pgfstrokecolor}{rgb}{0,0.19921875,0.3984375}P_{t_{n}}}\in{\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}}$,
$v_{x},v_{y},v_{z},\omega_{z}$ : control command to maneuver MAV;

procedure Global Planner

${\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}}\leftarrow<Q_{p},{\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}}>$

$S_{o}\leftarrow$
CheckingOccupiedSegments(${\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}},M_{t_{n}}$)

if $S_{o}>0$ then

for $i\leftarrow S_{o}$ do

$A_{i},b_{i}\leftarrow$ ParallelConvexDecomposition($S^{i}_{o}$)

$S_{*}^{i}\leftarrow$ FindPushingDirections($S^{i}_{o},A_{i},b_{i}$)

${\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}}\leftarrow$
CalculateGradients($S_{*}^{i},{\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}}$)

return
${\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}}\leftarrow$
ApplyBoxConstraintOptimization(${\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}}$)

procedure Local Planner

${\color[rgb]{0,0.19921875,0.3984375}\definecolor[named]{pgfstrokecolor}{rgb}{0,0.19921875,0.3984375}P_{t_{n}}}\leftarrow<Q_{p},{\color[rgb]{0,0.19921875,0.3984375}\definecolor[named]{pgfstrokecolor}{rgb}{0,0.19921875,0.3984375}P_{t_{n}}}>$

$C_{o}\leftarrow$
GetCloseInObstacles(${\color[rgb]{0,0.19921875,0.3984375}\definecolor[named]{pgfstrokecolor}{rgb}{0,0.19921875,0.3984375}P_{t_{n}}},M_{t_{n}}$)

return $<v_{x},v_{y},v_{z},\omega_{z}>\leftarrow$
ApplyNMPC(${\color[rgb]{0,0.19921875,0.3984375}\definecolor[named]{pgfstrokecolor}{rgb}{0,0.19921875,0.3984375}P_{t_{n}}},C_{o}$)

###  III-A Global Planner

The proposed approach consists of two planning stages: local and global. The
local planner is designed as a constraint nonlinear optimization problem
(NLP), specifically an NMPC. The computation time of NMPC increases when the
number of constraints increases, i.e., reference trajectory lies closer or
within the obstacles. Such behaviour can lead to local minima and cannot find
near-optimal control policy to avoid close-in obstacles. Hence, the global
planner does refine the initial reference trajectory in parallel with the
local planner to push the reference trajectory away from the obstacle zones.
Hence, the proposed global planner is formulated as follows:

$\displaystyle
J=\lambda_{smooth}J_{smooth}+\lambda_{obs}J_{obs}+\lambda_{feasibility}J_{feasibility},$
(1)

where $\lambda_{*},*\in\\{smooth,obs,feasibility\\}$ are weight parameters
were set as 0.2, 0.6, and 0.2, respectively. $\lambda_{smooth}$ and
$\lambda_{feasibility}$ were set to low values mainly due to relax smoothness
and feasibility adjustment compared to obstacle avoidance adjustment by
putting high penalty weight $\lambda_{obs}$. In the following sub-sections,
formulation of global planner components is explained adhering to Algorithm.
1.

####  III-A1 Finding Pushing Direction

Some of the control points in $R_{t_{n}}$ can occur within the obstacles
zones. Hence, control points that lie within the obstacle zone $O_{m}$ must be
pushed towards an obstacle-free zone $F_{m}$. Let $S_{o}^{i}\in R_{t_{n}}$ be
the $i^{th}$ segment within $R_{t_{n}}$ to be modified. $S_{o}^{i}$ consists
of a set of control points $\mathbf{c}_{j}\in S_{o}^{i},j=0,...,N^{seg}_{i}$,
where $N^{seg}_{i}$ is the number of control points in $S_{o}^{i}$. Thus,
pushing direction of each control point is determined by solving the following
convex problem for each segment:

$\displaystyle\min_{\mathbf{p}_{0},...,\mathbf{p}_{n}}$
$\displaystyle\lambda_{1}t_{1}+\lambda_{2}t_{2}+\lambda_{3}t_{3}$ (2) s.t.
$\displaystyle\quad A\mathbf{p}_{j}\leq b,$
$\displaystyle\left\|\mathbf{p}_{0}-\mathbf{c}_{0}\right\|_{2}\leq t_{1},$
$\displaystyle\left\|\mathbf{p}_{n}-\mathbf{c}_{n}\right\|_{2}\leq t_{2},$
$\displaystyle\sum_{k=1}^{n-1}\left\|\mathbf{p}_{k+1}-\mathbf{p}_{k}\right\|_{2}\leq
t_{3},$

where $A$ and $b$ represents the free space $F_{m}$ as a convex polyhedron
from $\mathbf{c}_{0}$ to $\mathbf{c}_{n}$ in $S_{o}^{i}$ (Sec.III-A2). For the
$i^{th}$ segment, $n=N^{seg}_{i}$. The regularization parameters:
$\lambda_{1}=0.8,\lambda_{2}=0.8$, and $\lambda_{3}=0.6$, were set in a way to
provide more bias on start and end control points compared to middle control
points. $\mathbf{p}_{0},...,\mathbf{p}_{N^{seg}_{i}}$ construct the updated
segment $S_{*}^{i}$ corresponding to $S_{o}^{i}$ that will be used for finding
each control point’s gradient direction (Sec. III-A3).

####  III-A2 Parallel Convex Decomposition

To further reduce the computation time, we have implemented the parallel
version of Convex Decomposition [5] (Algorithm 1, line 9). Once desired
control points are identified, i.e., $\mathbf{c}_{j}\in
S_{o}^{i},j=0,...,N^{seg}_{i}$ (Sec.III-A1), check for intermediate control
points that are in $F_{m}$. Convex decomposition is applied to such successive
control points in parallel that result in the free space in the form of H-rep
$Ax\leq b$ for each $S_{o}^{i}$.

####  III-A3 Calculating Gradients

The objective of $J_{obs}$ is to push each $S_{o}^{i},i=0,...,N^{seg}$ segment
towards the obstacle-free zone. $N^{seg}$ is the number of segments that are
within the obstacle zone for the considered refine trajectory segment
$R_{t_{n}}$, at time $t_{n}$. For the $i^{th}$ segment, by knowing
$S_{o}^{i}$, $S_{*}^{i}$ can be determined (III-A1). To find each gradient
direction vector that crosses the $\mathbf{c}_{j}\perp
S_{*}^{i},\;j=0,..,N^{seg}_{i}$, let
$\mathbf{v}_{1}=\mathbf{c}_{j+1}-\mathbf{c}_{j-1}$ be the approximated
direction vector along $\mathbf{c}_{j}$, and $\mathbf{p}_{k}\in S_{*}^{i}$ be
the control point that intersects $\mathbf{v}_{1}$ ( Fig. 4). Then, the
corresponding direction vector $\mathbf{v}_{2}$ can be defined as
$\mathbf{p}_{k}-\mathbf{c}_{j}$. By calculating angle
$\theta=cos^{-1}(\mathbf{v}_{1}\cdot\mathbf{v}_{2}/\left\|\mathbf{v}_{1}\right\|_{2}\left\|\mathbf{v}_{2}\right\|_{2})$
between $\mathbf{v}_{1}$ and $\mathbf{v}_{2}$, the optimal value of k can be
determined as provided in Algorithm 2. Thus, the gradient vector that
corresponds to $c_{j}$ can be fully determined as:

$\begin{split}&\mathbf{c}_{j}^{grad}=(\mathbf{c}_{j}^{*}-\mathbf{c}_{j})/\left\|\mathbf{c}_{j}^{*}-\mathbf{c}_{j}\right\|_{2},\\\
&\mathbf{c}^{*}_{j}=\mathbf{p}_{k}+\frac{(\mathbf{p}_{k}-\mathbf{p}_{k-1})(\mathbf{v}_{1}\cdot(\mathbf{c}_{j}-\mathbf{p}_{k}))}{\mathbf{v}_{1}\cdot(\mathbf{p}_{k}-\mathbf{p}_{k-1})}\end{split}.$
(3)

Algorithm 2 Estimation of direction vectors pushing the control points towards
the free space.

$\mathbf{Inputs}$: at time $t_{n}$,
${\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}}$:
reference trajectory to be refined, $N^{seg}$: segments indices to be refined
within
${\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}}$

$\mathbf{Outputs}$:
${\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}}$:
after adding, gradient vector corresponds to each control point

procedure GradientEstimation

for $i\leftarrow 0\quad to\quad N^{seg}$ do

for $j\leftarrow 1\quad to\quad N_{i}^{seg}$ do

$k\leftarrow N^{seg}_{i}/2$

$\mathbf{v}_{1}=\mathbf{c}_{j+1}-\mathbf{c}_{j-1},\;\mathbf{v}_{2}=\mathbf{p}_{k}-\mathbf{c}_{j}$

$\mathbf{c}_{j}\in S_{o}^{i},\;\mathbf{p}_{k}\in S_{*}^{i}$

$val=previous\\_val\leftarrow\mathbf{v}_{1}\cdot\mathbf{v}_{2}$

while $k\geq 0\quad and\quad k<N_{i}^{seg}$ do

$k\leftarrow\left\\{\begin{matrix}k--,\quad if\>val\leq 0\\\ k++,\quad
otherwise\end{matrix}\right.$

$val\leftarrow\mathbf{v_{1}}\cdot\mathbf{v_{2}}$

if $val\cdot previous\\_val\leq 0$ then

$\mathbf{c}^{*}_{j}\leftarrow\mathbf{p}_{k}+\frac{(\mathbf{p}_{k}-\mathbf{p}_{k-1})(\mathbf{v}_{1}\cdot(\mathbf{c}_{j}-\mathbf{p}_{k}))}{\mathbf{v}_{1}\cdot(\mathbf{p}_{k}-\mathbf{p}_{k-1})}$

$\delta d\leftarrow\left\|\mathbf{c}_{j}^{*}-\mathbf{c}_{j}\right\|_{2}$

$\mathbf{c}_{j}^{grad}=\frac{\mathbf{c}_{j}^{*}-\mathbf{c}_{j}}{\left\|\mathbf{c}_{j}^{*}-\mathbf{c}_{j}\right\|_{2}}$

return
${\color[rgb]{0.3984375,0,0.19921875}\definecolor[named]{pgfstrokecolor}{rgb}{0.3984375,0,0.19921875}R_{t_{n}}}$

Figure 4: Pushing control points that are within the obstacle zone, towards
the free space. Projected control points segment ($S_{*}^{i}$) is obtained as
explained in Sec. III-A1 utilizing $S_{o}^{i}$ for $i^{th}$ segment.
$c_{j}^{*}$ depicts gradient vector corresponding to $c_{j}$

Once gradient vectors are estimated, $J_{{obs}}$ is determined (4), which is
defined as a continuously differentiable exact penalty function.

$\begin{split}&J_{obs}=\Sigma_{i=d}^{N_{r}-d}J_{{obs}_{i}},\\\
&J_{{obs}_{i}}=\mathbf{v}_{i}\cdot dis_{e}^{3},\quad\frac{\partial
J_{{obs}_{i}}}{\partial\mathbf{c}_{i}}=-3\cdot
dis_{e}^{2}\cdot\mathbf{c}_{i}^{grad},\\\ \end{split}$ (4)

where
$dis_{e}=D_{z}-(\mathbf{c}_{i}-\mathbf{c}_{i}^{*})\cdot\mathbf{c}_{i}^{grad}$
and $\mathbf{v}_{i}=\mathbf{c}_{i+1}-\mathbf{c}_{i}$, and avoidance distance
$D_{z}$ was set to 0.8m (distance must be higher than the radius of the MAV)
in our study.

####  III-A4 Dead Zone Recovery

The map construction is not precise when the depth sensor has a small FoV.
Moreover, EDTM building takes a considerable amount of time when the
environment is cluttered. Therefore, the local planner may generate control
commands that lead to quadrotor $Q_{p}$ maneuvers into the $D_{z}$ (Fig. 3)
zone. In such situations, free space segmentation (Sec.III-A2) does not
provide correct constraints set $A,b$. Hence, the proposed approach (2) fails
to estimate control points $\mathbf{p}_{0},...,\mathbf{p}_{N_{i}^{seg}}$
appropriately, i.e., estimated control points may lie in the extreme ends
($\mathbf{c}_{0},\mathbf{c}_{N_{i}^{sec}}$) of the provided trajectory
$S_{o}^{i}$, provided that $R_{t_{n}}$ is not dynamically feasible. Hence, the
objective is to consider whole $R_{t_{n}}$ rather than each segment
separately, followed by the free space segmentation. Thus, the following
recovery mechanism is proposed to push the $R_{t_{n}}$ away from $O_{m}$. The
recovery mechanism is executed only when the (2) is failed.

$\displaystyle\min_{\mathbf{p}_{1},...,\mathbf{p}_{N_{r}}}$
$\displaystyle\sum_{l=1}^{N_{r}}q_{l}$ (5) s.t. $\displaystyle\quad
A_{r}(\mathbf{p}_{l}+\mathbf{c}_{l})\leq
b_{r},\;\left\|\mathbf{p}_{l}\right\|_{2}\leq q_{l},\;l=1,...,N_{r},$

where $c_{l},l=1,...,N_{r}$ are the control points to be pushed. The recovered
control points are determined by $\mathbf{p}_{l}+\mathbf{c}_{l}$, $N_{r}$ is
the number of control points at time $t_{n}$ in $R_{t_{n}}$, and $A_{r}$ and
$b_{r}$ are obtained by giving $R_{t_{n}}$ to Algorithm 1, line 9.

####  III-A5 Smoothing

We have employed a velocity controller since the proposed trajectory tracker
targets low-speed maneuvers. Hence, higher-order components, i.e,
acceleration, jerk, snap, should be minimized, which causes effects such as
vibrations. However, we decided to minimize only acceleration components
without considering higher-order components, e.g., jerk, snap. We have
formulated $J_{smooth}$ minimizing both acceleration and jerk components as
well as only considering acceleration components. However, adding jerk did not
affect $J_{smooth}$ considerably. Thus, $J_{smooth}$ was formulated only as
minimizing the acceleration components:

$\begin{split}&J_{{smooth}_{i}}=\mathbf{a}_{i}^{\top}\mathbf{a}_{i},\;\;\;\frac{\partial
J_{{smooth}_{i}}}{\partial\mathbf{c}_{i}}=2\frac{\partial\mathbf{a}_{i}}{\partial\mathbf{c}_{i}},\\\
\end{split}$ (6)

where
$\partial\mathbf{a}_{i}/\partial\mathbf{c}_{i}=1,\;\partial\mathbf{a}_{i}/\partial\mathbf{c}_{i+1}=-2,\;\partial\mathbf{a}_{i}/\partial\mathbf{c}_{i+2}=1$.
$\mathbf{a}_{i},\mathbf{v}_{i},\mathbf{c}_{i}\in\mathbb{R}^{3}$ are
respectively acceleration
($\mathbf{a}_{i}=\mathbf{c}_{i+2}-2\mathbf{c}_{i+1}+\mathbf{c}_{i}$), velocity
($\mathbf{v}_{i}=\mathbf{c}_{i+1}-\mathbf{c}_{i})$, and control point at
$i^{th}$ index of $R_{t_{n}}$.

####  III-A6 Feasibility

To ensure the refined trajectory, namely, $R_{t_{n}}$, which is dynamically
feasible for the maneuver, objective function penalizes the velocity and
acceleration components only when their limits exceed the min and max, as
follows:

$\begin{split}&J_{{feasibility}_{i}}=(\mathbf{v}_{i}\oplus\mathbf{v}_{max})^{\top}(\mathbf{v}_{i}\oplus\mathbf{v}_{max})\cdot\frac{1}{\delta^{2}}\\\
&\quad\quad\quad\quad\quad\quad+(\mathbf{a}_{i}\oplus\mathbf{a}_{max})^{\top}(\mathbf{a}_{i}\oplus\mathbf{a}_{max})\end{split}$
(7)

where the operator $\oplus$ is defined as

$\oplus=\left\\{\begin{matrix}-&if\;\mathbf{v}_{i}>\mathbf{v}_{max}\;||\;\mathbf{a}_{i}>\mathbf{a}_{max}\\\
+&if\;\mathbf{v}_{i}<-\mathbf{v}_{max}\;||\;\mathbf{a}_{i}<-\mathbf{a}_{max}\\\
not\;considering&otherwise\end{matrix}\right.,$ (8)

where allowed maximum velocity and acceleration components are given by
$\mathbf{v}_{max}\in\mathbb{R}^{3}$ and $\mathbf{a}_{max}\in\mathbb{R}^{3}$,
respectively. When the velocity and acceleration components are within the
allowed range, there will be no added cost. Once objective function $J$ (1)
was formed, we have used L-BFGS-B (Limited-memory Broyden Fletcher Goldfarb
Shanno Box-constrained algorithm) [29] for solving J. Subsequently, Mosek
solver [30] was employed to solve (2) and (5).

###  III-B Local Planner

At time $t_{n}$,
$P_{t_{n}}=[\mathbf{Q}_{p},\mathbf{c}_{t_{n}},\mathbf{c}_{t_{n}+1},...,\mathbf{c}_{t_{n}+N_{p}}]\subseteq
R_{t_{n}}$ forms the reference trajectory for the given prediction horizon,
$N_{p}$. The local planner generates the optimal control to maneuver the
quadrotor considering close-in obstacles $g_{2}(\mathbf{w})$ and system
dynamics $g_{1}(\mathbf{w})$ where
$\mathbf{w}=[\textbf{u}_{t_{n}},\ldots,\textbf{u}_{t_{n}+N_{p}-1},\textbf{x}_{t_{n}},\ldots,\textbf{x}_{t_{n}+N_{p}}]$.
Hence, the objective of local planner is to optimize both control inputs and
states simultaneously. Such an objective can be designed using multiple
shooting technique as follows:

$\displaystyle J_{P}(\textbf{x},\textbf{u})_{t_{n}}$
$\displaystyle=\sum_{l=0}^{N_{p}}{\left\|\textbf{x}_{t_{n}+l}-\textbf{c}_{t_{n}+l}\right\|_{Q}^{2}+\left\|\textbf{u}_{t_{n}+l}-\textbf{v}^{ref}_{t_{n}+l}\right\|_{R}^{2}}$
(9) $\displaystyle\min_{\mathbf{w}}$ $\displaystyle
J_{P}(\textbf{x},\textbf{u})_{t_{n}}$ s.t. $\displaystyle
g_{1}(\mathbf{w})=0,\quad g_{2}(\mathbf{w})\leq 0$
$\displaystyle\mathbf{x}_{min}\leq\textbf{x}_{t_{n}+l}\leq\mathbf{x}_{max}\quad\forall
0\leq l\leq N_{p}$
$\displaystyle-\mathbf{v}_{max}\leq\textbf{u}_{t_{n}+l}\leq\mathbf{v}_{max}\quad\forall
0\leq l\leq N_{p}-1,$

At every planning cycle, local planner gets $Q_{p},\mathbf{x}_{t_{n}}$, and
$\mathbf{u}_{t_{n}}$ as an input and estimates the optimal control policy,
i.e.,
$\mathbf{\hat{u}}_{t_{n}}=\\{\hat{v}_{t_{n}}^{x},\hat{v}_{t_{n}}^{y},\hat{v}_{t_{n}}^{z},\hat{\omega}_{t_{n}}^{z}\\}$,
where $\hat{v}_{t_{n}}^{\mu},\mu\in x,y,z$ denotes velocity on each $\mu$
direction, and yaw angle around z axis is given by $\hat{\omega}_{t_{n}}^{z}$.
The local planner is adopted from our previous work, where the explanation of
$g1$, $g2$ and $\mathbf{\hat{u}}_{t_{n}}$ is detailed [4].

To conclude, as summarized in Algorithm. 1, this section explained how the
proposed trajectory tracker is formulated. In the following section, the
qualitative and quantitative analysis of the proposed approach is provided.

##  IV EXPERIMENTAL PROCEDURE AND RESULTS

The experiment prototype of MAV DJI M100 (Fig. 5) is equipped with the
following components: Velodyne Lite 16 lidar for reasoning the MAV’s
surrounding environment; Nvidia Jetson NX computer for online computations.
Offline computations and the simulated experiments were carried out on an
Intel i9-9900K (16) @ 5 GHz computer. Hence, the timing breakdowns were
measured for both real-world and simulated experiments by those two computers.
The simulated experiments were performed in a Gazebo environment. For the
simulated and real-world experiments, PX4 [31] and DJI A3 controllers were
employed, respectively.

Figure 5: The experimental prototype MAV (DJI M100) was used in real-world
experiments. The proposed approach runs on the on-board computer (Nvidia
Jetson NX) and sends control commands to A3 controller

The first experiment222 tracking accuracy without considering obstacles:
https://www.youtube.com/watch?v=pKVeGdr8crU was aimed to estimate reference
trajectory tracking error without refining. Such trajectory tracking can be
directly used in, for example, cinematography. Moreover, this is a way to
check the local planner (9) is able to track the reference trajectory that the
proposed global planner provides. As shown in Fig. 6, we estimated position
estimation error $\left|p-p^{ref}\right|_{2}$ between the tracked trajectory
$p$ and the reference trajectory $p^{ref}$. The fusion of GPS position, IMU
data, and vehicle velocity is used to estimate $p$, whereas $p^{ref}$ is the
output of local planner. The mean estimation error
$\overline{\left|p-p^{ref}\right|}_{2}$ was less than 1m during the whole
flight in which velocity varied in between -1.2 m/s to 1.2 m/s.

Figure 6: Experimental results for real-world tracking accuracy without
considering obstacles. Estimated tracking error is less than 1m during the
whole experiment Figure 7: The trajectory tracking error for real-world
experimental results on tracking the reference trajectory shown in Fig. 6
TABLE I: Comparative analysis of the proposed approach and the three other
approaches for checking goal-reaching accuracy. All the metrics were obtained
during experiments on 12 different environments while keeping the same start
and goal poses

Algorithm | Success Fraction (SF) | Mean Computation Time (MCT) in seconds | Distance Estimation (m)
---|---|---|---
Mean | Max | Min
[15] RRT* max_allowed_iterations=10000 | 0.41 | 2.4 | 112.56 | 198.45 | 91.04
[4] $P_{t_{n}}=20,update\\_range=5m$ | 0.58 | 0.397 | 93.5 | 154.67 | 68.45
[4] $P_{t_{n}}=40,update\\_range=5m$ | 0.66 | 0.441 | 98.91 | 201.56 | 78.23
[9] $N_{whole}=N_{safe}=6$ $max\\_poly=3$ | 0.83 | $\approx 0.01$ | 54.56 | 81.45 | 49.39
[9] $N_{whole}=N_{safe}=12$ $max\\_poly=6$ | 0.83 | $\approx\textbf{ 0.01}$ | 53.78 | 78.67 | 47.45
Proposed $P_{t_{n}}=10,update\\_range=4m$ | 0.91 | $0.04\pm 0.01$ | 56.78 | 68.78 | 49.86
Proposed $P_{t_{n}}=15,update\\_range=6m$ | 1.0 | $\mathbf{0.03}\pm\textbf{0.01}$ | 54.89 | 66.80 | 48.67

  * 1

$N_{whole},N_{safe}$ : the number of discretization points in the whole and
safe trajectory, $max\\_poly$: maximum number of polydrons to represent the
free space, $P_{t_{n}}$: NMPC prediction horizon length

The second experiment was devoted to demonstrating the behaviour of the
proposed approach in a real-world condition333 behaviour of the proposed
approach in a challenging environment https://youtu.be/g6xHvkcrYcQ, where the
initial reference trajectory passes through a cluttered environment followed
by open space and back to a cluttered environment where the terminal pose was
placed in an obstacle zone. Map update range ($update\\_range$) was kept 4m
from the center of the MAV and max speed set to 0.6m/s for the safety of MAV.
Total flight time was around 150s and the distances of initial reference
trajectory and traversed trajectory were 54.3m and 79.8m, respectively. Since
trajectory termination pose was within the obstacles, trajectory tracker
terminates early(see Fig. 8). Such a behaviour is due the fact that the global
planner was designed as a box-constraint function minimizor, whereas local
planner was designed as a constraint NLP. Hence, the local planner terminated
correctly, though the global planner completely failed to refine, which is in
fact true.

Figure 8: Showcasing the behaviour of the proposed trajectory tracker in a
challenging environment

In the third experiment444 experiments used for estimating the run-time
https://www.youtube.com/watch?v=jyDe5BSigm8, we conducted four different real-
world tests to estimate the run-time breakdown (mean computation time) in the
average case. Three out of four tests were performed in static environments:
open area with small obstacles, open area with sizeable obstacles, and a
cluttered environment) and the fourth experiment was performed in a dynamic
environment ( Fig.9). Reference trajectories of each of them were completely
different from each other. However, we fixed the trajectory tracking duration
to 90s. Afterwards, run-time breakdown (Fig.10) was estimated based on three
sub-modules: NMPC solver (main force of the local planner), EDT mapper
(utilize both local and global planner), and main parts of the global planner
(smoothing, feasibility, calculating gradients, and finding pushing
directions). The objective was to understand how the run-time of each of the
listed sub-modules is affected due to environmental changes. Since those three
modules were executed in parallel, local and global planners have mean
computation times of approximately 0.06s (15Hz) and 0.05s (20Hz),
respectively.

TABLE II: Comparative analysis of the proposed approach and the three other
approaches for checking goal-reaching accuracy. All the metrics have been
obtained from experiments on 12 different environments while keeping the same
start and goal poses

Algorithm | Success Fraction (SF) | Mean Computation Time (MCT) in seconds | Distance Estimation (m)
---|---|---|---
Mean | Max | Min
[15] RRT* max_allowed_iterations=10000 | 0.41 | 2.4 | 112.56 | 198.45 | 91.04
[4] $P_{t_{n}}=20,update\\_range=5m$ | 0.58 | 0.397 | 93.5 | 154.67 | 68.45
[4] $P_{t_{n}}=40,update\\_range=5m$ | 0.66 | 0.441 | 98.91 | 201.56 | 78.23
[9] $N_{whole}=N_{safe}=6$ $max\\_poly=3$ | 0.83 | $\approx 0.01$ | 54.56 | 81.45 | 49.39
[9] $N_{whole}=N_{safe}=12$ $max\\_poly=6$ | 0.83 | $\approx\textbf{ 0.01}$ | 53.78 | 78.67 | 47.45
Proposed $P_{t_{n}}=10,update\\_range=4m$ | 0.91 | $0.04\pm 0.01$ | 56.78 | 68.78 | 49.86
Proposed $P_{t_{n}}=15,update\\_range=6m$ | 1.0 | $\mathbf{0.03}\pm\textbf{0.01}$ | 54.89 | 66.80 | 48.67

  * 1

$N_{whole},N_{safe}$ : number of discretization points in the whole and safe
trajectory, $max\\_poly$: maximum number of polydrons to represent the free
space, $P_{t_{n}}$: NMPC prediction horizon length

Figure 9:  Different scenarios for real-world experiments: static (a,b,d) and
dynamic (c) were used to estimate the run-time breakdowns of the proposed
trajectory tracker (Fig.10) Figure 10: Estimation of mean computation time
(run-time) of the proposed approach, i.e., time break down of each sub
components, in real-world scenarios (see Fig.9)

In the final experiment, we have generated 12 random forests, e.g., Fig.
11(a), where density (40m$\times$40m$\times$10m) was kept the same for all the
environments. The three other methods: RRT* [15], a local planner [4], and
FASTER [9] were used to validate the proposed approach. The results are
provided in Table II and an example test case is shown in Fig. 11. When the
environment is cluttered, FASTER failed mainly due to the inability to find a
path to local goal pose using JPS [32]. In the proposed approach, the dead
zone recovery technique tries to recover when the global planner fails to
refine the trajectory and the local planner is also capable of planning ahead
independently from the global planner. In consequence, the proposed approach
has a higher success rate (number of times successfully reach the goal)
compared to the other methods despite mean computation time (MCT) (ratio of
total execution time to the total number of iterations) is slightly lower than
FASTER. Since we are targeting low-speed maneuver, MCT is also acceptable. In
each environment, the same start and goal poses were considered and the
distance between them was set to 38.6m, ensuring no obstacle presence on those
poses. There is no distinctive difference in the mean distance estimation
between the proposed and FASTER.

Figure 11: An example of testing the proposed (b) and FASTER (c) algorithms on
a randomly generated forest (a)

##  V CONCLUSION

This work presents a reference trajectories tracking approach for low-speed
agile flights ensuring safety and dynamic feasibility in completely unknown
environments. The essential properties of the proposed approach are online
trajectory refinement and near-optimal control policy generation in parallel
in horizon-based fashion, while only reasoning the surrounding environment.
The proposed approach was tested on various simulated and real-world
environments, achieving long range trajectory tracking. The local and global
planners have mean computation times of approximately 0.06s (15Hz) and 0.05s
(20Hz), respectively, provided that tracking accuracy is less than 1m in
obstacle-free zones. We expect to extend this work for high-speed maneuvers in
which we are going to focus on improving the local planner. The source code
and complete experiments are available at Github444 The source code and
complete experiments - https://github.com/GPrathap/trajectory-tracker.git

## ACKNOWLEDGMENT

This research has been financially supported by The Analytical Center for the
Government of the Russian Federation (Agreement No. 70-2021-00143 dd.
01.11.2021, IGK 000000D730321P5Q0002))

## References

  * [1] T. Baca, D. Hert, G. Loianno, M. Saska, and V. Kumar, “Model predictive trajectory tracking and collision avoidance for reliable outdoor deployment of unmanned aerial vehicles,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2018, pp. 6753–6760.
  * [2] M.-E. Guerrero-Sánchez, O. Hernández-González, G. Valencia-Palomo, F.-R. López-Estrada, A.-E. Rodríguez-Mata, and J. Garrido, “Filtered observer-based ida-pbc control for trajectory tracking of a quadrotor,” _IEEE Access_ , vol. 9, pp. 114 821–114 835, 2021.
  * [3] O. Mechali, L. Xu, Y. Huang, M. Shi, and X. Xie, “Observer-based fixed-time continuous nonsingular terminal sliding mode control of quadrotor aircraft under uncertainties and disturbances for robust trajectory tracking: Theory and experiment,” _Control Engineering Practice_ , vol. 111, p. 104806, 2021\.
  * [4] G. Kulathunga, D. Devitt, and A. Klimchik, “Trajectory tracking for quadrotors: an optimization-based planning followed by controlling approach,” _doi.org/10.21203/rs.3.rs-963714/v2_.
  * [5] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar, “Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments,” _IEEE Robotics and Automation Letters_ , vol. 2, no. 3, pp. 1688–1695, 2017.
  * [6] A. Bircher, M. Kamel, K. Alexis, H. Oleynikova, and R. Siegwart, “Receding horizon” next-best-view” planner for 3d exploration,” in _2016 IEEE international conference on robotics and automation (ICRA)_. IEEE, 2016, pp. 1462–1468.
  * [7] H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, and E. Galceran, “Continuous-time trajectory optimization for online uav replanning,” in _2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2016, pp. 5332–5339.
  * [8] W. Merkt, V. Ivan, and S. Vijayakumar, “Continuous-time collision avoidance for trajectory optimization in dynamic environments,” in _2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2019, pp. 7248–7255.
  * [9] J. Tordesillas, B. T. Lopez, and J. P. How, “Faster: Fast and safe trajectory planner for flights in unknown environments,” in _2019 IEEE/RSJ international conference on intelligent robots and systems (IROS)_. IEEE, 2019, pp. 1934–1940.
  * [10] C. Liu, C.-Y. Lin, and M. Tomizuka, “The convex feasible set algorithm for real time optimization in motion planning,” _SIAM Journal on Control and optimization_ , vol. 56, no. 4, pp. 2712–2733, 2018.
  * [11] B. Landry, R. Deits, P. R. Florence, and R. Tedrake, “Aggressive quadrotor flight through cluttered environments using mixed integer programming,” in _2016 IEEE international conference on robotics and automation (ICRA)_. IEEE, 2016, pp. 1469–1475.
  * [12] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in _2011 IEEE International Conference on Robotics and Automation_. IEEE, 2011, pp. 2520–2525.
  * [13] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments,” in _Robotics Research_. Springer, 2016, pp. 649–666.
  * [14] G. Kulathunga, D. Devitt, R. Fedorenko, S. Savin, and A. Klimchik, “Path planning followed by kinodynamic smoothing for multirotor aerial vehicles (mavs),” in _2020 International Conference Nonlinearity, Information and Robotics (NIR)_. IEEE, 2020, pp. 1–7.
  * [15] G. Kulathunga, R. Fedorenko, S. Kopylov, and A. Klimehik, “Real-time long range trajectory replanning for mavs in the presence of dynamic obstacles,” in _2020 5th Asia-Pacific Conference on Intelligent Robot Systems (ACIRS)_. IEEE, 2020, pp. 145–153.
  * [16] M. W. Mueller, M. Hehn, and R. D’Andrea, “A computationally efficient motion primitive for quadrocopter trajectory generation,” _IEEE transactions on robotics_ , vol. 31, no. 6, pp. 1294–1310, 2015.
  * [17] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and efficient quadrotor trajectory generation for fast autonomous flight,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 4, pp. 3529–3536, 2019.
  * [18] S. Liu, N. Atanasov, K. Mohta, and V. Kumar, “Search-based motion planning for quadrotors using linear quadratic minimum time control,” in _2017 IEEE/RSJ international conference on intelligent robots and systems (IROS)_. IEEE, 2017, pp. 2872–2879.
  * [19] R. Tallamraju, E. Price, R. Ludwig, K. Karlapalem, H. H. Bülthoff, M. J. Black, and A. Ahmad, “Active perception based formation control for multiple aerial vehicles,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 4, pp. 4491–4498, 2019.
  * [20] V. Usenko, L. von Stumberg, A. Pangercic, and D. Cremers, “Real-time trajectory replanning for mavs using uniform b-splines and a 3d circular buffer,” in _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2017, pp. 215–222.
  * [21] J. Tordesillas, B. T. Lopez, J. Carter, J. Ware, and J. P. How, “Real-time planning with multi-fidelity models for agile flights in unknown environments,” in _2019 international conference on robotics and automation (ICRA)_. IEEE, 2019, pp. 725–731.
  * [22] B. T. Lopez and J. P. How, “Aggressive collision avoidance with limited field-of-view sensing,” in _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2017, pp. 1358–1365.
  * [23] X. Zhou, Z. Wang, H. Ye, C. Xu, and F. Gao, “Ego-planner: An esdf-free gradient-based local planner for quadrotors,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 478–485, 2020.
  * [24] M. Bangura and R. Mahony, “Real-time model predictive control for quadrotors,” _IFAC Proceedings Volumes_ , vol. 47, no. 3, pp. 11 773 – 11 780, 2014, 19th IFAC World Congress. [Online]. Available: http://www.sciencedirect.com/science/article/pii/S1474667016434890
  * [25] J. Ji, X. Zhou, C. Xu, and F. Gao, “Cmpcc: Corridor-based model predictive contouring control for aggressive drone flight,” _arXiv preprint arXiv:2007.03271_ , 2020.
  * [26] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard, “Octomap: An efficient probabilistic 3d mapping framework based on octrees,” _Autonomous robots_ , vol. 34, no. 3, pp. 189–206, 2013.
  * [27] H. Oleynikova, Z. Taylor, M. Fehr, R. Siegwart, and J. Nieto, “Voxblox: Incremental 3d euclidean signed distance fields for on-board mav planning,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2017.
  * [28] I. Ragnemalm, “The euclidean distance transform in arbitrary dimensions,” _Pattern Recognition Letters_ , vol. 14, no. 11, pp. 883–888, 1993.
  * [29] Lbfgs++. [Online]. Available: https://lbfgspp.statr.me/
  * [30] Mosek. [Online]. Available: https://www.mosek.com/
  * [31] L. Meier, D. Honegger, and M. Pollefeys, “Px4: A node-based multithreaded open source robotics framework for deeply embedded platforms,” in _2015 IEEE international conference on robotics and automation (ICRA)_. IEEE, 2015, pp. 6235–6240.
  * [32] K. Zhou, L. Yu, Z. Long, and S. Mo, “Local path planning of driverless car navigation based on jump point search method under urban environment,” _Future Internet_ , vol. 9, no. 3, p. 51, 2017.