1 Motivation, Problem Statement, Related Work
Among the criteria of designing autonomous quadrotors, generating optimized trajectories and tracking the flight paths precisely are two critical components in the action aspect. As shown in our recent work Teach-Repeat-Replan gao2020teach, a cascaded planning framework with global trajectory generation and local collision avoidance support agile flights under the user preferable routines. In work gao2020teach, though the first criteria is met by the global and local planners, the controller has no guarantees on tracking the generated motion precisely. Also, in industrial applications, the planner and controller of a quadrotor are mostly independently designed, making it hard to tune the joint performance in different applications.
Some works seo2019robust; li2020fast attempt to compensate uncertainties introduced by disturbances, by designing error-tolerated trajectory planning methods based on Hamilton-Jacobi Reachability Analysis bansal2017hamilton. These works set handcrafted disturbance bounds, making it too conservative to find a feasible solution among dense obstacles. Tal et.al, tal2018accurate propose control systems for accurate trajectory tracking that improves tracking accuracy. Nevertheless, they still try to track the unreachable trajectory when facing violent disturbance instead of adjusting the primary trajectory. If un-negligible disturbance occurs, local replanners such as zhou2019robust; usenko2017real can plan motions to rejoin the reference quickly, but they are inferior to give a proper temporal distribution. The closest work to this paper liniger2015optimization applies Model predictive contouring control (MPCC) lam2010model as the planner for miniature car racing, where safety constraints are established by modeling linear functions from the boundary of the racing track. However, such constraints are not directly available for a quadrotor in unstructured environments. What’s more, due to the limited planning horizon, MPCC cannot guarantee feasibility and heavily relies on proper parameter tuning.
To bridge this gap, we propose an efficient, receding horizon, local adaptive low-level planner as the middle layer between our original planner and controller. Our method is named as corridor-based model predictive contouring control (CMPCC) since it builds upon on MPCC lam2010model and utilizes the flight corridor as hard safety constraints. It optimizes the flight aggressiveness and tracking accuracy simultaneously, thus improving our system’s robustness by overcoming unmeasured disturbances. Our method features its online flight speed optimization, strict safety and feasibility, and real-time performance, and will be released111https://github.com/ZJU-FAST-Lab/CMPCC as a low-level plugin for a large variety of quadrotor systems.
2 Technical Approach
We get the global optimized reference trajectory and the flight corridor from our previous work Teach-Repeat-Replan gao2020teach, as shown in Fig. 1(a). Assuming a reference point moves along the global trajectory, we construct a receding horizon MPC with a linearized quadrotor dynamics. The optimization objective at the -th time-step in a horizon is shown in Fig. 1(b), where is the drone’s position, is the moving reference point, and is the speed of . The tracking error = . The objective trades off the minimization of and the maximization of in the predictive horizon .
Then we construct linear inequality constraints for . For a given reference point on the global trajectory, we define as the intersection of ’s normal plane with the corresponding polyhedron. As shown in Fig. 2(a), the resulting is a convex polygon. Then each edge of expands a plane sweeping along the direction of , which gives a polygon tube, as shown in Fig. 2(b). The inner side of this tube is considered as the safe space near and will be modeled as inequality constraints.
In addition to physical limits for each state, a terminal velocity constraint is added such that the terminal speed should be less than in the predictive horizon. Thus, feasibility can be guaranteed since the reference trajectory is globally optimal.
The global trajectory is parameterized by its reference time , where . Note that , where is definite according to the global trajectory but denoted by truly indicates the traveling progress. We model the system states with the position, velocity and acceleration of , and inputs as the jerk of . They are shown in 1 and 2 at the -th time-step.
We formulate the optimization problem as follows:
where in 3 is the weight of the progress of the reference point. Four kinds of constraints are introduced in the optimization: state-transfer equations governed by the -order integral model (4), lower and upper bounds of states and inputs (5 and 6), the polygon-tube constraints in 7, and the terminal velocity constraints in 8.
The optimization problem mentioned above is a typical QP, which is solved by OSQPosqp with warm start speed-up. In practice, we choose a predictive horizon and the sampling interval , which means . The performance of our algorithm is tested on an Intel i7-6700 CPU, with average solving time around 5.
4.1 Experiments configuration
We use a self-developed quadrotor with an Intel Realsense D435i stereo camera222https://www.intelrealsense.com/depth-camera-d435i/ and a DJI N3 flight controller333https://www.dji.com/n3
for state estimation. All modules run solely on a DJI Manifold 2-C onboard computer444https://www.dji.com/manifold-2. Our system inherits the localization, mapping and global planning from the Teach-Repeat-Replan system gao2020teach, where readers can check these modules in detail. The overall hardware and software architecture of our system is shown in Fig. 3(a) and Fig. 3(b).
4.2 Autonomous flight with contact disturbance
We apply challenging force disturbance to the drone to test the performance of the proposed CMPCC, as shown in Fig. 4(a). The global trajectory (blue), our locally re-planned trajectory (red), and the geometry constraints (magenta) after the hit are visualized in Fig. 4(b). As seen in the top-down view of the experiment in Fig. 5(a), oscillation occurres near the hit position, but the local trajectory soon converges to the global one. Also, as shown in Fig. 5(b), the instant force disturbance changes the temporal distribution of the optimal trajectory, resulting in the delaying of the trajectory of cmpcc (red) relative to which without disturbance (magenta) and the global trajectory (blue).
4.3 Autonomous flight with wind disturbance
We also test our method with wind disturbance by a fan, as shown in Fig. 6(a). Without the proposed CMPCC, the quadrotor tracks the global trajectory generated by Teach-Repeat-Replan with only a feedback controller, and collides with the nearby obstacle soon. However, thanks to the safety guarantee, the proposed CMPCC re-plans a safe trajectory and rejoins the global reference quickly under the wind disturbance, as shown in Fig. 6(b).
The video of the experiments is available. 555https://www.youtube.com/watch?v=_7CzBh-0wQ0
5 Main Experimental Insights
In practice, the flight performance of a quadrotor can be affected by many factors. Among all issues, the unexpected and unmeasurable disturbance is always an essential one for quadrotor autonomous navigation, especially for fast and aggressive flight. Recently, most autonomous quadrotor systems oleynikova2020open; gao2020teach are developed with several independent modules include controller, planner, and perception, with the assumption that a properly designed, smooth, derivative bounded trajectory can be tracked by a controller within high bandwidth. However, this assumption does not always hold. No matter how robust the feedback controller is, it’s noted that it may fail when encountering drastic disturbance, such as immediate contact and a gust of wind, which are demonstrated in our experiments. Traditionally, people have to spend tons of time tuning the parameters of the feedback controller until a satisfactory performance. In this work, as validated by our challenging experiments, the proposed intermediate low-level replanner successfully compensates disturbances by planning local safe trajectories and automatically adjusting the flight aggressiveness. Therefore, the robustness of fast autonomous flight is improved significantly. Moreover, thanks to the convex formulation, the proposed CMPCC is solved within 5 , which suits onboard usage well.
In experiments, we also observe that the polygon tube now we use heavily depends on the static corridor. Therefore it cannot handle the variation of the environment or dynamic obstacles. In the future, we plan to investigate the way to generate safety constraints for CMPCC online.