CCMPCC: Corridor-based Model PredictiveContouring Control for Aggressive Drone Flight
Jialin Ji* , Xin Zhou* , Chao Xu , Fei Gao Among the criteria of designing autonomous quadrotors, generating optimizedtrajectories and tracking the flight paths precisely are two critical components in theaction aspect. As shown in our recent work
Teach-Repeat-Replan [2], a cascadedplanning framework with global trajectory generation and local collision avoidancesupport agile flights under the user preferable routines. In work [2], though the firstcriteria is met by the global and local planners, the controller has no guarantees ontracking the generated motion precisely. Also, in industrial applications, the plannerand controller of a quadrotor are mostly independently designed, making it hard totune the joint performance in different applications.Some works [7, 4] attempt to compensate uncertainties introduced by distur-bances, by designing error-tolerated trajectory planning methods based on Hamilton-Jacobi Reachability Analysis [1]. These works set handcrafted disturbance bounds,making it too conservative to find a feasible solution among dense obstacles.
Talet.al, [9] propose control systems for accurate trajectory tracking that improvestracking accuracy. Nevertheless, they still try to track the unreachable trajectorywhen facing violent disturbance instead of adjusting the primary trajectory. If un-negligible disturbance occurs, local replanners such as [11, 10] can plan motionsto rejoin the reference quickly, but they are inferior to give a proper temporal dis-tribution. The closest work to this paper [5] applies Model predictive contouringcontrol (MPCC) [3] as the planner for miniature car racing, where safety constraintsare established by modeling linear functions from the boundary of the racing track.However, such constraints are not directly available for a quadrotor in unstructuredenvironments. What’s more, due to the limited planning horizon, MPCC cannotguarantee 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. Ourmethod is named as corridor-based model predictive contouring control (CMPCC)since it builds upon on MPCC [3] and utilizes the flight corridor as hard safety con-straints. 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 feasibil-ity, and real-time performance, and will be released as a low-level plugin for a largevariety of quadrotor systems. College of Control Science and Engineering, Zhejiang University. * Equal contributors.
E-mail: { jlji, iszhouxin, cxu, and fgaoaa } @zju.edu.cn https://github.com/ZJU-FAST-Lab/CMPCC a r X i v : . [ c s . R O ] J u l J. Ji, X. Zhou, C. Xu, F. Gao.
We get the global optimized reference trajectory and the flight corridor from ourprevious work
Teach-Repeat-Replan [2], as shown in Fig. 1(a). Assuming a refer-ence point moves along the global trajectory, we construct a receding horizon MPCwith a linearized quadrotor dynamics. The optimization objective at the k -th time-step in a horizon is shown in Fig. 1(b), where s ( k ) is the drone’s position, p ( k ) isthe moving reference point, and v ( k ) p is the speed of p ( k ) . The tracking error e ( k ) = | p ( k ) − s ( k ) | . The objective trades off the minimization of { e ( k ) } and the maximiza-tion of { v ( k ) p } in the predictive horizon N . (a) 𝑠 (𝑘) 𝑒 (𝑘) 𝑣 𝑝(𝑘) 𝑝 (𝑘) (b) Fig. 1: (a) Flight corridors (b) Components of the objective of CMPCCThen we construct linear inequality constraints for s ( k ) . For a given referencepoint p ( k ) on the global trajectory, we define Ω as the intersection of v p ’s normalplane Φ 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 thedirection of v ( k ) p , which gives a polygon tube, as shown in Fig. 2(b). The inner side ofthis tube is considered as the safe space near p ( k ) and will be modeled as inequalityconstraints. Φ Ω 𝑣 𝑝 (a) Ω 𝑣 𝑝 (b) Fig. 2: Sequence of linear inequality constraints for safetyIn addition to physical limits for each state, a terminal velocity constraint is addedsuch that the terminal speed should be less than v ( N ) p in the predictive horizon. Thus,feasibility can be guaranteed since the reference trajectory is globally optimal. orridor based Model Predictive Contouring Control for Aggressive Drone Flight 3 The global trajectory p µ ( θ ) is parameterized by its reference time θ , where µ ∈ { x , y , z } . Note that v p = ∂ p ∂θ · ∂θ∂ t , where ∂ p ∂θ is definite according to the globaltrajectory but ∂θ∂ t denoted by v θ truly indicates the traveling progress. We model thesystem states with the position, velocity and acceleration of x , y , z , θ , and inputs asthe jerk of x , y , z , θ . They are shown in 1 and 2 at the k -th time-step. x ( k ) = [ x , v x , a x , y , v y , a y , z , v z , a z , θ , v θ , a θ ] T , (1) u ( k ) = [ j x , j y , j z , j θ ] T , (2)We formulate the optimization problem as follows: J = min x , u N ∑ k = (cid:26) ∑ µ = x , y , z (cid:16) µ ( k ) − p µ ( θ ( k ) ) (cid:17) − q · v ( k ) θ (cid:27) , (3) s . t . x ( k + ) = A d x ( k ) + B d u ( k ) , k = , , , ..., N − , (4) x l ≤ x ( k ) ≤ x u , k = , , , ..., N − , (5) u l ≤ u ( k ) ≤ u u , k = , , , ..., N − , (6) C ( k ) · [ x ( k ) , y ( k ) , z ( k ) ] T ≤ b ( k ) , k = , , , ..., N − , (7) | v ( N ) s | ≤ v ts , s = x , y , z , (8)where q in 3 is the weight of the progress of the reference point. Four kinds ofconstraints are introduced in the optimization: state-transfer equations governed bythe 3 rd -order integral model (4), lower and upper bounds of states and inputs (5 and6), 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 byOSQP[8] with warm start speed-up. In practice, we choose a 1 s predictive horizonand the sampling interval ∆ t = . s , which means N =
20. The performance of ouralgorithm is tested on an Intel i7-6700 CPU, with average solving time around 5 ms . We use a self-developed quadrotor with an Intel Realsense D435i stereo camera and a DJI N3 flight controller for state estimation. All modules run solely on a DJIManifold 2-C onboard computer . Our system inherits the localization, mappingand global planning from the Teach-Repeat-Replan system [2], where readers can check these modules in detail. The overall hardware and software architecture ofour system is shown in Fig. 3(a) and Fig. 3(b). On-board Computer (a)
Depth and grayscale imagesImu data Global mappingState estimationFlight corridor generationGlobal planningFlight tunnel generationCMPC C Low-level SO3 controller
Modules based on Teach-Repeat-ReplanCMPC C planner (b) Fig. 3: The hardware and software architecture of the UAV (a) (b)
Fig. 4: Circumstance of instant force disturbance x [m] -8-6-4-20 y [ m ] Spatial profile instant force disturbance (a) t [s] -8-6-4-202 y [ m ] Time profile globalcmpcc-disturbancecmpcc-no-disturbance instant force disturbance (b)
Fig. 5: Spatial and time profile facing instant force disturbanceWe apply challenging force disturbance to the drone to test the performance ofthe proposed CMPCC, as shown in Fig. 4(a). The global trajectory (blue), our locallyre-planned trajectory (red), and the geometry constraints (magenta) after the hit are orridor based Model Predictive Contouring Control for Aggressive Drone Flight 5 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 tothe global one. Also, as shown in Fig. 5(b), the instant force disturbance changesthe temporal distribution of the optimal trajectory, resulting in the delaying of thetrajectory of cmpcc (red) relative to which without disturbance (magenta) and theglobal trajectory (blue).
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 gener-ated by
Teach-Repeat-Replan with only a feedback controller, and collides with thenearby obstacle soon. However, thanks to the safety guarantee, the proposed CM-PCC re-plans a safe trajectory and rejoins the global reference quickly under thewind disturbance, as shown in Fig. 6(b). fan (a) x [m] -1.8-1.6-1.4-1.2-1-0.8-0.6-0.4-0.20 y [ m ] globaltrrcmpccwind (b) Fig. 6: The circumstance of wind disturbanceThe video of the experiments is available. In practice, the flight performance of a quadrotor can be affected by many fac-tors. Among all issues, the unexpected and unmeasurable disturbance is always anessential one for quadrotor autonomous navigation, especially for fast and aggres-sive flight. Recently, most autonomous quadrotor systems [6, 2] are developed withseveral independent modules include controller, planner, and perception, with theassumption that a properly designed, smooth, derivative bounded trajectory can betracked by a controller within high bandwidth. However, this assumption does notalways hold. No matter how robust the feedback controller is, it’s noted that it may J. Ji, X. Zhou, C. Xu, F. Gao. fail when encountering drastic disturbance, such as immediate contact and a gustof wind, which are demonstrated in our experiments. Traditionally, people have tospend tons of time tuning the parameters of the feedback controller until a satis-factory performance. In this work, as validated by our challenging experiments, theproposed intermediate low-level replanner successfully compensates disturbancesby planning local safe trajectories and automatically adjusting the flight aggressive-ness. Therefore, the robustness of fast autonomous flight is improved significantly.Moreover, thanks to the convex formulation, the proposed CMPCC is solved within5 ms , which suits onboard usage well.In experiments, we also observe that the polygon tube now we use heavily de-pends on the static corridor. Therefore it cannot handle the variation of the environ-ment or dynamic obstacles. In the future, we plan to investigate the way to generatesafety constraints for CMPCC online. References
1. Somil Bansal, Mo Chen, Sylvia Herbert, and Claire J Tomlin. Hamilton-jacobi reachability:A brief overview and recent advances. In , pages 2242–2253. IEEE, 2017.2. Fei Gao, Luqi Wang, Boyu Zhou, Xin Zhou, Jie Pan, and Shaojie Shen. Teach-repeat-replan: Acomplete and robust system for aggressive flight in complex environments.
IEEE Transactionson Robotics , 2020.3. Denise Lam, Chris Manzie, and Malcolm Good. Model predictive contouring control. In , pages 6137–6142. IEEE, 2010.4. Zhichao Li, Omur Arslan, and Nikolay Atanasov. Fast and safe path-following control usinga state-dependent directional metric. arXiv preprint arXiv:2002.02038 , 2020.5. Alexander Liniger, Alexander Domahidi, and Manfred Morari. Optimization-based au-tonomous racing of 1: 43 scale rc cars.
Optimal Control Applications and Methods , 36(5):628–647, 2015.6. Helen Oleynikova, Christian Lanegger, Zachary Taylor, Michael Pantic, Alexander Millane,Roland Siegwart, and Juan Nieto. An open-source system for vision-based micro-aerial ve-hicle mapping, planning, and flight in cluttered environments.
Journal of Field Robotics ,37(4):642–666, 2020.7. Hoseong Seo, Donggun Lee, Clark Youngdong Son, Claire J Tomlin, and H Jin Kim. Robusttrajectory planning for a multirotor against disturbance based on hamilton-jacobi reachabil-ity analysis. In , pages 3150–3157. IEEE, 2019.8. Bartolomeo Stellato, Goran Banjac, Paul Goulart, Alberto Bemporad, and Stephen Boyd.OSQP: An operator splitting solver for quadratic programs.
Mathematical Programming Com-putation , 2020.9. Ezra Tal and Sertac Karaman. Accurate tracking of aggressive quadrotor trajectories usingincremental nonlinear dynamic inversion and differential flatness. In , pages 4282–4288. IEEE, 2018.10. Vladyslav Usenko, Lukas von Stumberg, Andrej Pangercic, and Daniel Cremers. Real-timetrajectory replanning for mavs using uniform b-splines and a 3d circular buffer. In , pages 215–222. IEEE, 2017.11. Boyu Zhou, Fei Gao, Jie Pan, and Shaojie Shen. Robust real-time uav replanning using guidedgradient-based optimization and topological paths. arXiv preprint arXiv:1912.12644arXiv preprint arXiv:1912.12644