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.READ FULL TEXT VIEW PDF
Real-time, guaranteed safe trajectory planning is vital for navigation i...
We integrate sampling-based planning techniques with funnel-based feedba...
External disturbance forces caused by nonlinear springy electrical cable...
Autonomous robots that are capable of operating safely in the presence o...
Steerable needles are capable of accurately targeting difficult-to-reach...
We present Neural-Swarm2, a learning-based method for motion planning an...
Recently, quadrotors are gaining significant attention in aerial
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.
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  and tube-based MPC  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  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  or with prior information of a force field . 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  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 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 . The onboard experiments demonstrate the performance of our system in volatile real-world environments.
Accurate estimation and prediction of external forces assist robust control 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  or a Gaussian mixture field 
. For more generic conditions with unknown types of external forces, different force estimators are employed. Extended Kalman Filter (EKF) is applied in 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 , momentum-based estimator , 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. directly add an estimated force to the dynamic model. Other methods model the disturbance as a zero-mean, bounded disturbance  with handcrafted bound modification when the drone encounters sudden winds, or as Gaussian uncertainty distributions propagated with 3 confidence ellipses . 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 , we can apply a tighter bounded noise as the uncertainty of external force into the model.
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.  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  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 to generate large convex polytopes directly on an occupancy map.
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. 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  for a sufficiently short duration. The external force is defined as , where the bounded noise , is the maximum bound.
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. With a more precious estimation of the external forces by VID-Fusion
, a less conservative error FRSs could be obtained by employing a tighter variance bound.
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  of these ellipsoids is denoted by
where , are the shape matrices of ellipsoids, denotes the real symmetric positive definite matrices.
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  with the converted discrete-time model. The error initial shape matrix is updated with the outer approximate sum as .
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  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 , 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
For robust reference tracking and yaw control, while reducing the control efforts, we penalize the cost with the following three terms.
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.
In the aspect of control efforts, the penalty of the control input should be added as
is the weighting matrix of control input 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.
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).
The reference collision-free path we mention above is generated based on a kinodynamic hybrid-state A* proposed in , 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.
To ensure the robustness and accuracy of the estimation, a tightly-coupled Visual-Inertial-Dynamics estimator (VID-Fusion)  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  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.
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.
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.
We use a code generation tool Forces Pro  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  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 .
The real-world test is presented by an autonomous quadrotor platform in , 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 .
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 
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.
|Kamel et al. ||1.00||15.67||18.11|
|Hoseong et al. ||1.00||15.73||19.03|
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.
Secondly, we compare our method against a state-of-art local planner by Zhou  under different external forces for real-time systems. We also test the disturbance-aware controller  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.
|Zhou et al. ||0.70||7.65||14.65|
|Zhou + Kamel||0.50||10.93||19.18|
|Zhou + Kamel||0.40||12.67||19.33|
|Zhou + Kamel||0.45||9.95||20.01|
|Zhou + Kamel||0.25||9.04||19.13|
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 forceis 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.
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 .
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.
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.
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.
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.
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.
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.
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.