External Forces Resilient Safe Motion Planning for Quadrotor

Adaptive autonomous navigation with no prior knowledge of extraneous disturbance is of great significance for quadrotors in a complex and unknown environment. The mainstream that considers external disturbance is to implement disturbance-rejected control and path tracking. However, the robust control to compensate for tracking deviations is not well-considered regarding energy consumption, and even the reference path will become risky and intractable with disturbance. As recent external forces estimation advances, it is possible to incorporate a real-time force estimator to develop more robust and safe planning frameworks. This paper proposes a systematic (re)planning framework that can resiliently generate safe trajectories under volatile conditions. Firstly, a front-end kinodynamic path is searched with force-biased motion primitives. Then we develop a nonlinear model predictive control (NMPC) as a local planner with Hamilton-Jacobi (HJ) forward reachability analysis for error dynamics caused by external forces. It guarantees collision-free by constraining the ellipsoid of the quadrotor body expanded with the forward reachable sets (FRSs) within safe convex polytopes. Our method is validated in simulations and real-world experiments with different sources of external forces.


page 1

page 5

page 6

page 7

page 8


FaSTrack: a Modular Framework for Real-Time Motion Planning and Guaranteed Safe Tracking

Real-time, guaranteed safe trajectory planning is vital for navigation i...

KDF: Kinodynamic Motion Planning via Geometric Sampling-based Algorithms and Funnel Control

We integrate sampling-based planning techniques with funnel-based feedba...

Manipulation Planning to Keep an Object Stable under a Sequence of External Forces

We present a planner for a robot to keep an object stable under a sequen...

A Reliable Gravity Compensation Control Strategy for dVRK Robotic Arms With Nonlinear Disturbance Forces

External disturbance forces caused by nonlinear springy electrical cable...

Robust-RRT: Probabilistically-Complete Motion Planning for Uncertain Nonlinear Systems

Robust motion planning entails computing a global motion plan that is sa...

A Novel Shaft-to-Tissue Force Model for Safer Motion Planning of Steerable Needles

Steerable needles are capable of accurately targeting difficult-to-reach...

Neural-Swarm2: Planning and Control of Heterogeneous Multirotor Swarms using Learned Interactions

We present Neural-Swarm2, a learning-based method for motion planning an...

Code Repositories

I Introduction

Safe trajectory generation under real-world conditions where quadrotors are exposed to external disturbance and other sources of uncertainty has been a crucial concern, especially in complex and dynamic environments [7, 2, 14]. External forces caused by air turbulence, loaded objects, or other uncertain disturbance induce a considerable effect on the quadrotor system and challenge the stability of planning.

Fig. 1: An indoor flight when the quadrotor enters a wind zone. (a) The wind pushes the quadrotor to obstacles while our planner replans a new feasible reference (green dotted) to adapt it. (b) Visualizations. The predicted states are generated by NMPC and the references are created by path searching. Video is at: https://youtu.be/nSKbzAM0v18

Existing attempts mainly focus on robust control with unpredictable disturbance to prevent the quadrotor from deviating to its desired trajectories. Disturbance-aware controllers for quadrotors like robust MPC [10] and tube-based MPC [20] aim to follow a predefined reference path with robustness guarantees and try to compensate for the influence of the disturbance. However, these methods either may fail to handle extremely large forces because of the controller’s limitations or generate too conservative trajectories because they consider all sources of uncertainty. Another work [8] addresses this issue by adding a receding horizon, corridor-based, local adaptive tracking layer between the planner and the controller. When the quadrotor encounters an external force, this method directly replans a new trajectory on the current state without any estimation of this force. Due to its lack of modeling on the external force, it cannot handle sustained forces and occasionally generates an unsafe path. Other works partly solve this problem by planning under known constant wind [14] or with prior information of a force field [15]. These methods partially simplify the environments and need reliable predictions of the external force. As a result, it is impractical in real-world situations with dynamic and unexpected external disturbances.

In this work, we propose a systematic framework to achieve robust local planning, which accounts for the influence of extreme external disturbance111The source code will be released at: https://github.com/ZJU-FAST-Lab/forces_resilient_planner. The presented method searches a kinodynamic feasible path as a rough reference with added nominal force estimated by the method in [3] to generate motion primitives. Then we formulate an optimal control problem (OCP) for trajectory generation based on the reference path, which encodes the external force with its uncertainty. This OCP can be solved by a corridor-based NMPC that exploits the full dynamics and geometrical shape of a quadrotor. We extend the ellipsoidal approximation of FRSs[19] which are the state sets of error dynamics in a planning horizon. Therefore, we can constrain the discrete-time FRSs in a safe flight corridor for collision avoidance. The real-world experiments illustrate the robustness of our method and an indoor flight is shown in Fig.1.

Our contributions can be summarized as:

  • An efficient front-end kinodynamic path searching and collision checking method with external forces consideration.

  • An online trajectory optimization algorithm that utilizes nonlinear model predictive control with safe ellipsoid boundaries constrained in a convex flight corridor to enforce reliable obstacle avoidance.

  • A (re)planning framework for quadrotor that integrates the VID-Fusion external force estimator [3]. The onboard experiments demonstrate the performance of our system in volatile real-world environments.

Ii Related Work

Ii-a Modeling of External Forces

Accurate estimation and prediction of external forces assist robust control[18] and offset-free trajectory tracking for a quadrotor. Some works attempt to present this special external force by modeling the disturbance as a predicted spatiotemporally varying wind field [15] or a Gaussian mixture field [16]

. For more generic conditions with unknown types of external forces, different force estimators are employed. Extended Kalman Filter (EKF) is applied in

[10] to estimate the external disturbance while utilizing the same model used in the inner-loop attitude dynamics to decrease the tracking errors. Other approaches like nonlinear observer [22], momentum-based estimator [18], and optimization-based methods [3, 17] are also widely used for visual-inertial odometry (VIO) systems and force estimation.

To further include the external force into a quadrotor system, Kamel et al.[10] directly add an estimated force to the dynamic model. Other methods model the disturbance as a zero-mean, bounded disturbance [19] with handcrafted bound modification when the drone encounters sudden winds, or as Gaussian uncertainty distributions propagated with 3 confidence ellipses [9]. These approaches can guarantee the safety and robustness of a quadrotor system but lack the flexibility to handle excessive disturbance. With an external force estimated in [3], we can apply a tighter bounded noise as the uncertainty of external force into the model.

Ii-B Robust Planning with Disturbance

Researchers have focused on OCP of tracking a given reference path with robustness consideration to handle uncertainties and disturbances. MPC is an efficient tool to generate optimal local trajectories while encodes uncertainty and obstacle avoidance. Although linear MPC demonstrates its excellent performance on trajectory generation and navigation [8, 1], nonlinear MPC of full dynamics is better regarding disturbance rejection, especially for aggressive trajectory tracking[10, 9].

The effects of external forces require a quadrotor system to apply higher-level strategies to resist disturbances and plan a safe and energy-efficient path. Among all attempts for robust path planning, Singh et al. [20] generate a conservative trajectory subject to bounded disturbances by pre-computing a globally valid invariant tube, while others [7, 16] conduct global planning assuming that a quadrotor is flying in a region of known time-varying winds. However, such methods rely on the accuracy of disturbance predictions and modeling, which are not practicable for real-world planning. To solve these limitations, we incorporate kinodynamic path searching and NMPC local planning to generate safe trajectories with real-time estimated force. In terms of obstacle avoidance and free space representation, the common method represents the obstacles by flexible parameterization [19] and incorporates in cost functions with the distance to the nearest obstacle. Another representation of free space in an environment generates a safe flight corridor of several convex polytopes  [6, 13, 23] to constrain the trajectory. Our method also exploits such representation[23] to generate large convex polytopes directly on an occupancy map.

Iii Planning With External Forces

Iii-a Quadrotor Dynamic Model

We consider the dynamic model with accurate control of Euler angles, which assumes that its rate can accurately track the desired command. The system state is , where denote the position and velocity of the quadrotor. , , are the roll, pitch and yaw angles. The control input is in which are the command rates of the Euler angles, is thrust command of the quadrotor in body frame. We provide a full-body nonlinear dynamic model as follows:


where is the rotation matrix parameterized by the Euler angles, is the magnitude of gravitational acceleration, is the mass of the quadrotor. We can represent the nonlinear dynamics as . Components of the rotor drag and drag like effects are complicated to model and correlated with quadrotor’s velocity, we consider it in the system to compensate its interference to disturbance bound[11]. To further simplify its aerodynamics, we apply a first-order drag model by adding the drag force in the xy body frame. We define , where is the drag coefficient constant, then the drag force in the world frame is


Instead of predicting the external forces or modeling the field of special force like winds, we generally obtain a real-time force estimation and then adjust the planning strategy when the force surpasses its bound. The disturbance of external forces can be expressed as a nominal value with an additive bounded noise, which makes it possible to design an NMPC that satisfies the constraints under bounded disturbance. The nominal force can be treated as a constant value calculated in [3] for a sufficiently short duration. The external force is defined as , where the bounded noise , is the maximum bound.

Iii-B Collision Avoidance Constraints

Iii-B1 Error Dynamics

We construct a linearized system for closed-loop dynamics of error state around nominal states , inputs and nominal external force . Assuming that the linearization error can be neglected around nominal states with the condition that the feedback control policy can be expressed as , is the feedback gain. We can get the error dynamics as


where , , , .

In order to propagate the uncertainty of external forces on each nominal state, we apply the analytic solution based on Hamilton-Jacobi (HJ) reachability analysis that quantitatively expresses the error FRS as approximated ellipsoids[19]. With a more precious estimation of the external forces by VID-Fusion[3]

, a less conservative error FRSs could be obtained by employing a tighter variance bound.

Iii-B2 Propagation of Safe Ellipsoid Boundary

To further solve the problem, we discrete the dynamics with a sampling time over time steps, as . For convenience, we denote the discrete-time state as at stage , . The rotation matrix is , the error FRS is , and the external force is denoted as .

We inflate the geometrical shape of a quadrotor to ensure the safety of our generated trajectory. The quadrotor is modeled as an ellipsoid with a radius and a height . Therefore, we can represent ego geometrical ellipsoid at stage as


which is centered on quadrotor’s position with a shape matrix as


As shown in Fig.2, the safe ellipsoid boundary is an outer ellipsoid that includes the shape of a quadrotor and ellipsoidal approximation of error FRS , where are the shape matrices accordingly. Given two ellipsoids centered on the same point, the optimal outer ellipsoidal approximation which is guaranteed to contain the Minkowski sum [12] of these ellipsoids is denoted by


where , are the shape matrices of ellipsoids, denotes the real symmetric positive definite matrices.

Fig. 2: The inner ellipsoid (pink) is the ellipsoidal approximation of the error forward reachable set, the middle one (blue) is the geometrical modeling of the quadrotor. The outer ellipsoid (purple) is the ellipsoidal approximated sum.

Therefore, the shape matrix of the approximated safe ellipsoid boundary is obtained by . Because we use the discrete representation of the model, we propagate in each stage with current error initial shape matrix and system matrix of quadrotor rather than using the same initial state during the planning horizon . The max outer ellipsoid propagation law that includes the external force can be computed by


Note that we only concern the uncertainty of quadrotor’s position in the total error ellipsoid for further obstacle avoidance, so we directly block the shape matrix to the first 3-dimension matrix, where denotes the first block of the matrix. The maximal ellipsoidal reachable set of error dynamics is individually computed by [19] with the converted discrete-time model. The error initial shape matrix is updated with the outer approximate sum as .

Iii-B3 Safe Corridor Constraints

With the propagated safe ellipsoid boundary in each stage, we can encode collision avoidance constraints by restricting the ellipsoid in a convex collision-free corridor. To ensure sufficient freedom for NMPC to get a better and refined trajectory, we use the method in [23] to rapidly generate a series of convex polytopes to partially cover reference waypoints in the current planning horizon, as shown in Fig.3. The convex polyhedron can be represented as linear constraints with and , . Inspired by the computation of the maximum volume inscribed ellipsoid in a polytope [21], we have


It is equal to solve the problem


This optimization problem can be easily solved analytically, so that the collision avoidance constraint can be obtained by

Fig. 3: Safety constraints by restricting the safe ellipsoids into convex polytopes. For each stage, the safe ellipsoid boundary shown in Fig. 2.

Iii-C Objective Functions

For robust reference tracking and yaw control, while reducing the control efforts, we penalize the cost with the following three terms.

Iii-C1 Navigation Cost

In each stage, we minimize the tracking deviation from the reference position to the predicted one. Other references as velocity and acceleration are relaxed during the optimization. A yaw angle sequence is pre-computed according to the orientation of reference velocity of front-end searched path. We define the stage cost as


where and are weighted squared norm induced by the and matrices. The terminal cost is


where and are the terminal weighting matrices. When the remained reference state is beyond its planning horizon, the addictive velocity in the final stage is penalized to satisfy the terminal status for quadrotor.

Iii-C2 Control Input Cost

In the aspect of control efforts, the penalty of the control input should be added as


is the weighting matrix of control input cost.

Iii-C3 Control Input Rate Cost

For the smoothness of the control input, we also penalize the variations between current and previous inputs for avoiding oscillatory trajectories, as


is the weighting matrix of the smoothness cost.

Iii-D External Forces Resilient NMPC

Finally, we can formulate the receding horizon optimization problem with nominal external force as


where is the initial state, and in (15e) are computed and propagated by equation (5) - (7). To reduce the problem’s complexity, is pre-computed before each optimization using the last loop planned series of Euler angles. Upper and lower bounds for control inputs and states should also be considered in (15f).

Iii-E Path Searching with Nominal Force

The reference collision-free path we mention above is generated based on a kinodynamic hybrid-state A* proposed in [24], which searches a kinodynamic feasible trajectory in an occupancy map that minimizes time duration and control cost. We extend the simplified kinematic model with added nominal external force for dynamically feasible primitives generation, which is better for computational efficiency compared with full nonlinear dynamics. This makes reference local path reliable under the specific external force instead of searching a global path which may collide if the system suffers such force. The front-end trajectory can be represented in three dimensions as time-parameterized piece-wise polynomials , . The state is , and the control input is . Hence, the state-space model is


The system equation is easy to get the closed-form solution with the initial state and input. The collision-free path in the planning phase is naturally close to the obstacles and may become unsafe due to the variance of the external force. Therefore, rather than the exact path following, we only consider the feasible path as a rough reference for corridor generation and orientation guiding.

Iv Implementation Details

Iv-a External Force Estimation

To ensure the robustness and accuracy of the estimation, a tightly-coupled Visual-Inertial-Dynamics estimator (VID-Fusion) [3] is used to optimize the pose and external force simultaneously via nonlinear optimization, which is suitable for our adaptive planning framework. We exploit the estimated force in [3] as the sum of nominal external force and drag force, and then redefine the external force as a resultant force in the world frame except for rotor thrust, gravity, and drag force in our system.

Iv-B System Overview of the (Re)planning Framework

Iv-B1 External Forces Adaptation

We apply kinodynamic A* to search a time-parameterized feasible path with a constant nominal force for the reference waypoints generation. Because the update frequency of external force is much higher than replanning frequency, we introduce a noise bound to withstand the deviation of estimated external force in acceptable time duration. The back-end nonlinear MPC considers the error states as a series of ellipsoidal forward reachable sets, admitting external force to vary within this bound during the tracking phase. Therefore the followed waypoints and the optimized predicted states are guaranteed to be safe under such external forces.

Iv-B2 Replanning Activation

The replanning strategy is event-triggered under the feasible consideration of the external force and reference path, which combines the real-time external force both in front-end and back-end planning, as shown in Fig.4. When the variance of external force is upper the noise bound or too fierce for NMPC to solve an acceptable solution, the front-end reference needs to replan to better suit the current force. In the second place, with the update of the environment, if the global target or the reference path collides, or the time-indexed reference is beyond a planning horizon to reach or far away to follow, then the replanning is triggered to generate a new path.

Fig. 4: The system overview diagram of the planning framework with the external force estimator. The VID-Fusion estimates the VIO and the external force simultaneously, and our planning module checks this time-varying external force. If the force is within its noise bound, the back-end NMPC can adjust for safe trajectory generation; otherwise, it triggers the replanning and reset reference path.

V Results

V-a Setups

We use a code generation tool Forces Pro [4] to solve our NMPC problem and directly use its generated C library for speed-up. We apply a receding horizon with and total time steps . The average solving time is around 5 ms on an Intel i7-10700 CPU computer for simulation test and within 10 ms on our quadrotor platform. The noise bound of the external force is set as according to our experiment data, to resist its allowable deviation during the planning phase.

The RotorS MAVs simulator [5] is employed for simulated flight tests. As shown in Fig.5, we separately make two scenarios of benchmark comparison. The first comparison is tracking conducted with an elliptical reference trajectory, while the second is the comparison of the whole planning framework. For further evaluations and analysis, we define a mass-normalized external force acceleration obtained by VID-Fusion, and a command acceleration calculated by command thrust and attitude of the quadrotor. We set the maximum velocity of the quadrotor as 2 .

Fig. 5: The experiment settings for comparison testing. (a) The test for reference tracking. A predefined reference trajectory is provided, which is partially in the wind zone. (b) The test for local planning. With a global target, the drone aims to fly through the wind zone.

The real-world test is presented by an autonomous quadrotor platform in [3], with an onboard computer (i7-8550U), a stereo camera (Intel Realsense D435), a DJI N3 controller and a rotor speed measurement unit. The radius and height of our quadrotor is and . The drag coefficient is identified as using model in (2). The force estimation setup follows the pipeline in [3].

V-B Simulation Tests

Fig. 6: Comparison of flying through a wind zone. The odometry is colored with the norm of command acceleration in y axis . In the red zone, the quadrotor suffers an external force . In Zhou’s method, the quadrotor does not consider the deviation of its position to the original planning trajectory due to external force and with a high possibility to collide if close to obstacles. Added with Kamel’s NMPC controller, it smooths the influence by an external force, but the estimator using EKF has some delay for the quadrotor to react. The right pictures are our method without and with external force. It is obvious that if the quadrotor encounters the external force, command will increase for disturbance resistance.

V-B1 Comparison of Reference Tracking

We firstly evaluate our external forces resilient NMPC with other MPCs [10, 19] with disturbance consideration under the same environments. Because other MPC-based methods require the predefined path and information of obstacles, for fairness to all MPCs, we provide a pre-computed global path for tracking and implement a disturbance observer for external force estimation. Method in [19]

solves the MPC by unscented dynamic programming and needs more than 70 ms computation time as they show. Since they do not provide open source applicable code for real-time solving, we re-implement it by using Forces Pro and provide available information of obstacles and wind zone for benchmark comparison. We define success rate as a successful flight without any collision to obstacles and the energy cost evaluated by squares of total commanded accelerations. The simulation environment is setting as Fig.

5 (1). When the quadrotor enters the range of the wind zone, it suffers from an external force or to simulate different extends of the winds.

Time (s) Energy
Kamel et al. [10] 1.00 15.67 18.11
Hoseong et al. [19] 1.00 15.73 19.03
Proposed 1.00 14.89 16.34
Kamel - - -
Hoseong - - -
Proposed 0.60 15.02 27.38
TABLE I: Comparison of Reference Tracking

The results are shown in Tab.I. If the wind is within the variance range and reaction window for tracking, all MPC methods can successfully finish the flight test. When the external force surpasses the bound, their methods fail to handle the disturbance while following the trajectory.

V-B2 Comparison of Planning Framework

Secondly, we compare our method against a state-of-art local planner by Zhou [25] under different external forces for real-time systems. We also test the disturbance-aware controller [10] using planning trajectory from Zhou’s method as a reference to assess the robustness of this controller. The scenario is as the map in Fig.5 (2). Within the wind zone, the quadrotor is under a constant force with different values and directions.

Time (s) Energy
Zhou et al. [25] 0.70 7.65 14.65
Zhou + Kamel 0.50 10.93 19.18
Proposed 1.00 8.33 13.71
Zhou 0.60 8.80 14.70
Zhou + Kamel 0.40 12.67 19.33
Proposed 1.00 8.17 16.81
Zhou 0.60 8.29 18.86
Zhou + Kamel 0.45 9.95 20.01
Proposed 1.00 8.27 16.37
Zhou - - -
Zhou + Kamel 0.25 9.04 19.13
Proposed 0.90 8.86 12.78
TABLE II: Comparison of Planning Framework

As the Tab.II

illustrates, when the external force is sufficiently small, the controller can resist such influence to a certain degree. As the external force increases, the local planner, without consideration of force disturbance, has a higher collision probability. A simulation comparison instance under force

is shown in Fig.6. The local planner, without force consideration, cannot recompense the trajectory offset in the y-axis resulting from an external force. Furthermore, with a disturbance-aware observer, the robust controller cannot give real-time feedback to the planner to adjust the trajectory because the current reference is already infeasible. The EKF based disturbance observer that is applied by controller smooths the external force. Hence, the controller performs delayed reactions to suddenly imposed force and sometimes will adversely increase the collision risk. For other cases in Tab.II, when the external force is not along with the quadrotor’s motion, the safe and successful flight under external force cost more energy, or the quadrotor can take advantage of external forces.

V-C Real-world Tests

V-C1 Indoor Flights Through Wind Zones

We present several indoor experiments in cluttered environments with strong winds in Fig.1. The quadrotor flies through an unknown narrow hallway fully autonomously with onboard computation and limited field of view (FOV) sensors. Several fans are set up near the possible flight paths, providing the wind zone for external disturbances. The speed of wind zones caused by fans ranges from to , measured by an anemometer. The wind causes the maximum external force at around to the quadrotor based on experiment data, measured by VID-Fusion [3].

The trajectory of a flight test through two different wind zones is shown in Fig.7. When the quadrotor enters wind zones, the increasing estimated external force triggers its replanning, which results in the variation of acceleration in the y-axis and adjustment of starting predicted state in back-end optimization.

Fig. 7: Real-world indoor Test. (a) The overall trajectory of the indoor flight experiment, colored with the norm of command acceleration . (b) The estimated force and command acceleration in the y-axis with x-position.

V-C2 Indoor Benchmark Flights Comparison

The comparison experiment is conducted in the same indoor flight environment mentioned above, where several wind zones are presented. With the same settings and a target point, Zhou’s local planner is tested in such variable conditions.

Fig. 8: Benchmark flight test for comparison. The blue curve is the estimated position by VINS, and the red doted curve is the command send by Zhou’s planner over time. The red vertical marked that the quadrotor is entering a wind zone.

As command positions and estimated positions are shown in Fig. 8, the planner fails to compensate position deviations mainly in this case and continues to follow an intractable trajectory after entering the wind zone, then collides with obstacles in our test.

V-C3 Indoor Flights With An Unmodeled Load

Flying with an object of unknown mass is also an essential application of delivery for quadrotors. We add a package to the quadrotor without modeling of complete dynamic model or measurement of a loaded object. A quadrotor (1.13 kg) with a loaded bottle (0.19 kg) is experimentally tested in a cluttered environment. When the quadrotor is hovering, the gravity of this load causes an equivalent mass normalized force acceleration in the z-axis.

Fig. 9: Flight test with an unmodeled load. (a) Snapshots of indoor flight test with a loaded bottle. (b) The norm of estimated force and command acceleration in the z-axis with x-position, compared with gravity effect of the load.

The snapshot is shown in Fig.9 (1), and the estimated external force and command acceleration in the z-axis of the quadrotor is shown Fig.9 (2). It illustrates that the planning acceleration commands sent by our planner compensate the influence of external force, which ensures a stable trajectory in the z-direction. The value of the external force matches the gravity effect of the loaded bottle and slightly varies during the whole flight.

In our experiment, the load is connected to the quadrotor with a short string so that the pendulum effect can be neglected in path planning. If the string of a suspended load is considered, the planner needs to model its dynamics and adds obstacles avoidance of the load.

V-C4 Outdoor Flights Under Natural Winds

To validate the robustness and efficiency of the proposed planning framework under natural winds, we conduct outdoor experiments in a cluttered forest with wind speed up to . Given several targets inside the forest, the drone generates a smooth trajectory to follow the targets against the natural wind gusts and the uncertain disturbance from leaves, as shown in Fig.10. It reaches a maximum velocity around . The outdoor flight test shows that our proposed framework has the capability of autonomous navigation in a completely unknown and complex environment with extreme natural disturbance.

Fig. 10: The snapshot of the outdoor flight test.

Vi Conclusion

This paper presents a systematic (re)planning framework for a quadrotor autonomous system in the presence of fierce external forces. We make a step forward to consistently include the external force in both the planning and tracking systems by incorporating the force in front-end path searching and back-end NMPC optimization. Using the ellipsoidal approximation of error FRSs and quadrotor’s inflated geometrical shape, we can guarantee collision avoidance by constraining the safe ellipsoid boundaries into a flight corridor along the reference path. The proposed approach can achieve real-time adaptive planning without offline computation of FRSs or the pre-computed collision-free reference trajectory. The benchmark comparisons in both simulations and real-world tests illustrate the necessity and adaptability of the proposed framework with external force considerations. The indoor flights with different external forces and outdoor flights with natural winds validate the robustness of the proposed method under different environments.


  • [1] M. Bangura and R. Mahony (2014) Real-time model predictive control for quadrotors. IFAC Proceedings Volumes 47 (3), pp. 11773–11780. Note: 19th IFAC World Congress External Links: ISSN 1474-6670, Document Cited by: §II-B.
  • [2] K. Cole and A. M. Wickenheiser (2018) Reactive trajectory generation for multiple vehicles in unknown environments with wind disturbances. IEEE Transactions on Robotics 34 (5), pp. 1333–1348. External Links: Document Cited by: §I.
  • [3] Z. Ding, T. Yang, K. Zhang, C. Xu, and F. Gao (2020) VID-fusion: robust visual-inertial-dynamics odometry for accurate external force estimation. External Links: 2011.03993 Cited by: 3rd item, §I, §II-A, §II-A, §III-A, §III-B1, §IV-A, §V-A, §V-C1.
  • [4] A. Domahidi and J. Jerez (2014–2019) FORCES professional. Note: Embotech AG, url=https://embotech.com/FORCES-Pro Cited by: §V-A.
  • [5] F. Furrer, M. Burri, M. Achtelik, and R. Siegwart (2016) Robot operating system (ros): the complete reference (volume 1). A. Koubaa (Ed.), pp. 595–625. External Links: ISBN 978-3-319-26054-9, Document, Link Cited by: §V-A.
  • [6] F. Gao, L. Wang, B. Zhou, X. Zhou, J. Pan, and S. Shen (2020) Teach-Repeat-Replan: a complete and robust system for aggressive flight in complex environments. IEEE Transactions on Robotics 36 (5), pp. 1526 – 1545. Cited by: §II-B.
  • [7] J. A. Guerrero, J. A. Escareno, and Y. Bestaoui (2013) Quad-rotor mav trajectory planning in wind fields. In 2013 IEEE International Conference on Robotics and Automation, Vol. , pp. 778–783. External Links: Document Cited by: §I, §II-B.
  • [8] J. Ji, X. Zhou, C. Xu, and F. Gao (2020) CMPCC: corridor-based model predictive contouring control for aggressive drone flight. External Links: 2007.03271 Cited by: §I, §II-B.
  • [9] M. Kamel, J. Alonso-Mora, R. Siegwart, and J. Nieto (2017) Robust collision avoidance for multiple micro aerial vehicles using nonlinear model predictive control. In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vol. , pp. 236–243. External Links: Document Cited by: §II-A, §II-B.
  • [10] M. Kamel, M. Burri, and R. Siegwart (2017) Linear vs nonlinear mpc for trajectory tracking applied to rotary wing micro aerial vehicles. IFAC-PapersOnLine 50 (1), pp. 3463–3469. Note: 20th IFAC World Congress External Links: ISSN 2405-8963, Document Cited by: §I, §II-A, §II-A, §II-B, §V-B1, §V-B2, TABLE I.
  • [11] S. Kim, D. Falanga, and D. Scaramuzza (2018) Computing the forward reachable set for a multirotor under first-order aerodynamic effects. IEEE Robotics and Automation Letters 3 (4), pp. 2934–2941. External Links: Document Cited by: §III-A.
  • [12] A. B. Kurzhanski (1997) Ellipsoidal calculus for estimation and feedback control. In Systems and Control in the Twenty-First Century, C. I. Byrnes, B. N. Datta, C. F. Martin, and D. S. Gilliam (Eds.), Boston, MA, pp. 229–243. External Links: ISBN 978-1-4612-4120-1 Cited by: §III-B2.
  • [13] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar (2017) Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments. IEEE Robotics and Automation Letters 2 (3), pp. 1688–1695. External Links: Document Cited by: §II-B.
  • [14] T. G. McGee and J. K. Hedrick (2006) Path planning and control for multiple point surveillance by an unmanned aircraft in wind. In 2006 American Control Conference, Vol. , pp. 6 pp.–. External Links: Document Cited by: §I, §I.
  • [15] M. Otte, W. Silva, and E. Frew (2016) Any-time path-planning: time-varying wind field + moving obstacles. In 2016 IEEE International Conference on Robotics and Automation (ICRA), Vol. , pp. 2575–2582. External Links: Document Cited by: §I, §II-A.
  • [16] L. Palmieri, T. P. Kucner, M. Magnusson, A. J. Lilienthal, and K. O. Arras (2017) Kinodynamic motion planning on gaussian mixture fields. In 2017 IEEE International Conference on Robotics and Automation (ICRA), Vol. , pp. 6176–6181. External Links: Document Cited by: §II-A, §II-B.
  • [17] T. Qin, P. Li, and S. Shen (2018) VINS-mono: a robust and versatile monocular visual-inertial state estimator. IEEE Transactions on Robotics 34 (4), pp. 1004–1020. External Links: Document Cited by: §II-A.
  • [18] F. Ruggiero, J. Cacace, H. Sadeghian, and V. Lippiello (2014) Impedance control of vtol uavs with a momentum-based external generalized forces estimator. In 2014 IEEE International Conference on Robotics and Automation (ICRA), Vol. , pp. 2093–2099. External Links: Document Cited by: §II-A.
  • [19] H. Seo, D. Lee, C. Y. Son, C. J. Tomlin, and H. J. Kim (2019) Robust trajectory planning for a multirotor against disturbance based on hamilton-jacobi reachability analysis. In 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vol. , pp. 3150–3157. External Links: Document Cited by: §I, §II-A, §II-B, §III-B1, §III-B2, §V-B1, TABLE I.
  • [20] S. Singh, A. Majumdar, J. Slotine, and M. Pavone (2017) Robust online motion planning via contraction theory and convex optimization. In 2017 IEEE International Conference on Robotics and Automation (ICRA), Vol. , pp. 5883–5890. External Links: Document Cited by: §I, §II-B.
  • [21] B. Stephen and V. Lieven (2004) Convex optimization. Cambridge university press. Cited by: §III-B3.
  • [22] B. Yüksel, C. Secchi, H. H. Bülthoff, and A. Franchi (2014) A nonlinear force observer for quadrotors and application to physical interactive tasks. In 2014 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, Vol. , pp. 433–440. External Links: Document Cited by: §II-A.
  • [23] X. Zhong, Y. Wu, D. Wang, Q. Wang, C. Xu, and F. Gao (2020) Generating large convex polytopes directly on point clouds. External Links: 2010.08744 Cited by: §II-B, §III-B3.
  • [24] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen (2019) Robust and efficient quadrotor trajectory generation for fast autonomous flight. IEEE Robotics and Automation Letters 4 (4), pp. 3529–3536. Cited by: §III-E.
  • [25] X. Zhou, Z. Wang, C. Xu, and F. Gao (2021) EGO-planner: an esdf-free gradient-based local planner for quadrotors. IEEE Robotics and Automation Letters 6, pp. 478–485. Cited by: §V-B2, TABLE II.