Appendix for the Motion Primitives-based Path Planning for Fast and Agile Exploration Method
AAppendix for the Motion Primitives-based Path Planning for Fast andAgile Exploration Method
Mihir Dharmadhikari, Tung Dang, Kostas Alexis
Abstract — This manuscript presents enhancements on ourmotion-primitives exploration path planning method for agileexploration using aerial robots. The method now furtherintegrates a global planning layer to facilitate reliable large-scale exploration. The implemented bifurcation between localand global planning allows for efficient exploration combinedwith the ability to plan within very large environments, whilealso ensuring safe and timely return-to-home. A new set ofsimulation studies and experimental results are presented todemonstrate the new improvements and enhancements. Themethod is available open-source as a Robot Operating System(ROS) package.
I. INTRODUCTIONResearch in autonomous robotic exploration and mappingof unknown environments is expanding into an ever increas-ing set of application domains. Pushing the frontier withrespect to the settings and environments within which robotscan be utilized as explorers [1–6], first responders [7, 8], andinspectors [9–12], aerial vehicles, in particular, are currentlyemployed in a multitude of civilian and military applications.Nevertheless, despite the unprecedented progress in the do-main and the multiple exploration strategies proposed [13–21], the current state-of-the-art, as demonstrated experimen-tally, is limited to rather low-speed conservative missions asthe robots try to guarantee safe navigation and simultaneousoptimized selection of subsequent exploration moves giventheir real-time onboard localization and mapping capabilities.However, low-speed exploration prohibits the exploitation ofthe full flight envelope and agility of Micro Aerial Vehicles(MAVs) and forbids large-scale exploration given the gener-ally limited battery life available in such platforms. Althoughhistorically this was justified due to the limitations of theonboard localization and mapping process, recent progress inrobotic perception paves new capabilities for fast and agileexploration if the overall robot autonomy functionality canexploit them.Motivated by the discussion above, we recently presentedour contribution of motion primitives-based fast explorationpath planning using aerial robots. In this appendix we presentthe extension of the method to allow for global re-positioningtowards identified frontiers of the exploration space, aswell as to perform auto-homing when the remaining batterynecessitates. The description provided is aligned with ouropen-source contribution available at https://github.com/unr-arl/mbplanner_ros . The authors are with the Autonomous Robots Lab, Universityof Nevada, Reno, 1664 N. Virginia, 89557, Reno, NV, USA [email protected]
The rest of this manuscript is organized as follows.Section II outlines the full exploration planning problem,followed by an outline of the enhanced planning procedurein Section III. New simulation studies are presented inSection IV, while additional experiments focusing on theglobal planning functionality are detailed in Section V.Finally, short conclusions are provided in Section VI.II. PROBLEM STATEMENTThe problem of agile exploration path planning in sub-terranean environments, as considered in this work, is aninstance of volumetric exploration of unknown space. Assuch, it aims to build a complete map of an environment forwhich no prior knowledge exists. Generally, a subterraneanenvironment is a closed, bounded and connected point setwith possible exceptions being access points from aboveground (e.g., mine portals, subway entrances, doors to facil-ities with underground floors). It consists of a large-in-scalecomplex network of branches, junctions, multiple levels andopenings (e.g., rooms or caves) and may involve a collectionof obstacles within it. As such, these kinds of environments(e.g., underground mines, tunnels, subway infrastructure,cave networks) with their distinct characteristics in lengthand topology introduce multiple challenges that must beaccounted for in the planner. In particular, they are typicallyvery large, and the areas to be navigated through are oftenconfined to tube-like tunnels and branching points. Thescale of the environment demands fast exploration and atthe same time prohibits planning from taking place in acomputationally efficient manner in the full configurationspace of the environment bounds. Nevertheless, the identifiedgeometric properties enable a key planning architecture,namely to “break” the problem into a “local” one, duringwhich the robot searches around its area to find paths thatcontinue its exploration, and a “global” one that is triggeredwhen the robot has to backtrack to a previous branching point(or other frontier) to continue its exploration from there.Given this two-layer approach, we define the concepts of“local completion” and “global completion” for the local andglobal planners respectively.Let M be a D occupancy map of the environment whichis incrementally built from measurements of an onboarddepth sensor S , as well as robot poses derived from alocalization system O responsible to fuse exteroceptive andproprioceptive data to simultaneously estimate the robotpath and the map of its environment. The map consistsof voxels m of three categories, namely m ∈ M free , m ∈ M occupied , or m ∈ M unknown representing free a r X i v : . [ c s . R O ] D ec V free ), occupied ( V occ ), and unknown ( V unm ) space re-spectively. Furthermore, let d max be the effective range,and [ F H , F V ] be the field-of-view in horizontal and verticaldirections respectively of the depth sensor S . In addition,let the robot’s configuration at time t be defined as thecombination of D position, linear velocity and heading ξ t = [ x t , y t , z t , v x , v y , v z , ψ t ] . Importantly, since for mostrange sensors’ perception stops at surfaces, sometimes hol-low spaces or narrow pockets cannot be fully exploredthus leading to a residual map M ∗ ,res ⊂ M unknown withvolume V ∗ ,res which is infeasible to explore given the robot’sconstraints. As a result, given a volume V ∗ , the potentialvolume to be explored is V ∗ ,explored = V ∗ \ V ∗ ,res . Definition 1 (Local Completion)
Given a map M , withina local sub-space M L of dimensions D L centered aroundthe current robot configuration, the planner reports “localcompletion” if V D L ,explored = V D L \ V D L ,res . Definition 2 (Global Completion)
Given the full occupancymap M of the environment with dimensions D G and vol-ume V D G , the planner considers “global completion” if V D G ,explored = V D G \ V D G ,res .In practice, it is not possible and unrealistic to identify V res , but completion can be approximated by the lack ofa collision-free path inside a planning volume which leadsto a space with potentially unknown volume larger than athreshold V δ . The local and global planner problems areformulated as follows. Problem
Given an occupancymap M and a local subset of it M L centered around thecurrent robot configuration ξ , find a collision-free andtraversability-aware (when applicable) path σ L = { ξ i } toguide the robot towards unmapped areas and maximize anexploration gain defined as the volume which is expectedto be mapped when the robot traverses along the path σ L with a sensor S . A path is admissible if it is collision-free in the sense of not colliding with D obstacles in themap and respecting the robot dynamic model. When “localcompletion” is reported by this planner, the global planneris to be engaged.
Problem
Given the exploredand unknown subsets of an occupancy map M of theenvironment and the current robot configuration ξ , finda dynamics-aware and collision-free path σ G leading therobot towards the frontiers of the unmapped areas. Feasiblepaths of this planning problem must take into account theremaining endurance of the robot. When the environmentis explored completely (“global completion”) or the batterylimits are approaching, find a collision-free path σ H to returnthe robot to its home location ξ home .III. METHOD ENHANCEMENTSSubterranean settings typically involve long and confinedcorridors connected by multi-way intersections. Such en-vironments therefore correspond to major challenges forrobotic exploration due to the scale (e.g., km in length) and geometric complexity. In response to these facts, thiswork proposes a bifurcated local-global exploration pathplanning method, called “Motion primitivies-based explo-ration path planner” (MBPlanner), that at its core exploitsmotion primitives-based planning to identify fast and agilepaths, while also detecting frontiers of the explored subsetof the environment towards which the robot can backtrackwhen needed to continue its exploratory mission. The methodis an improved version of our previous publication onMBPlanner [22] and is now released as an open-sourcecontribution. In further detail, the local motion primitives-based exploration planner, responsible for finding paths tomaximize the amount of volume mapped, searches within alocal space of fixed dimension to enable fast computationand the derivation of agile and efficiently exploring trajec-tories that exploit the aggressive dynamics of MAVs. Theexploration planner plans paths in D enabling explorationof environments with varying altitude. Due to the geometricparticularities of underground settings in terms of scale,complexity and topology, the local planner might reach adead-end or other scenario that prohibits the derivation ofan effective exploration path. The method is thus extendedto incorporate a global planner used to derive a) a safe andtimely return-to-home path, and b) paths towards previouslyidentified frontiers of the exploration space. An example useof the global planner is that after a robot has explored themain drift of the underground mine, it reaches a dead-end(mine heading) and thus has to return to a previously foundbranching point in order to continue its mission. The overalldiagram of the proposed solution is shown in Figure 1. Thelocal planning mode is detailed in [22], whereas the globalplanning functionality is analogous to the one presented inour previous contribution on subterranean exploration [23].
Fig. 1. Block diagram view of the proposed planner architecture, calledMBPlanner, involving the local layer of motion primitives-based explorationand its global layer responsible for auto-homing and repositioning towardsfrontiers of the exploration space when a successful local exploration pathis not possible to be identified.
IV. SIMULATION STUDIESA set of simulation studies were conducted in orderto evaluate and fine-tune the enhanced motion primitives-based exploration planner and its local and global planninglayers. First of all, the local exploratory planning stage wasevaluated through two simulation studies inside a) a subwaystation, and b) an underground mine. The simulation studiesere conducted using the RotorS Simulator [24], while thelocal planning window of the planner (the volume withinwhich the motion primitives-based tree is built) is set to D L of length × width × height = × × m and the robot bound-ing box D R is considered equal to length × width × height = . × . × . m. Both simulation studies were conducted as-suming a quadrotor micro aerial vehicle model integrating aLiDAR sensor with [ F H , F V ] = [360 , ◦ and d max = 50 mand the exploration speed was set to m/s. This test, depictedin Figure 2, indicates the ability of the proposed method toexplore both large-scale environments such as undergroundmines and buildings such as the subway station. Note thatthe subway station contained multiple levels requiring goingup floors through the openings in stairways demonstratingthe D exploration capability of the method.
Fig. 2. Simulation-based evaluation of the local stage of MBPlanner bothin a subway station and in an underground mine. Selected paths: 1) take-offand initial maneuver, 2) ascending to the next level through the opening ofa staircase, 3) also ascending to the next level, 4) handling an intersectioninside a mine. This study focuses on the more complex D explorationcapabilities of the method.
A third simulation study was further conducted with thegoal to evaluate and verify the whole global and localplanning functionalities of the proposed planner. In thissimulation study the environment used was an undergroundroom-and-pillar mine. This room-and-pillar mine environ-ment is multiple km-long and presents an array of multi-way intersections that challenge the planner’s behavior. Theperformance of the proposed planner is compared against thereceding horizon Next-Best-View Planner (NBVP) [17] andthe Frontiers exploration algorithm (FrontierPlanner) [14]implemented for 3D environments and combined with anoptimal sampling-based motion planner for collision-freenavigation to the frontiers [25].The room-and-pillar simulated environment consists oftwo sections, left and right, with the first being relativelymore spacious with respect to the width of its corridors andthe second rather more constrained. Hence, for a fair compar-ison between these three planners, two exploration scenarios are employed corresponding to the left and the right sectionsof the mine. A quadrotor model, similar to a real roboticsystem is developed in ROS-Gazebo utilizing the RotorSsimulator [24]. In both sections of the mine, each planner isrun 5 times for 15 minutes each (a typical flight time for ourflying platform). The average velocity of the robot is set to m/s. Relevant simulation results presenting the performanceof a) MBPlanner, b) NBVP, c) FrontierPlanner in the leftand right subsets of this environment are shown in Figure 3.Furthermore, the statistical comparison data with respect tothe exploration rates of each method are depicted in Figure 4.As it can be seen, MBPlanner outperforms the other twoapproaches. More specifically, in the left mine, all threeplanners present good exploration behavior, which could beattributed to the fact that the environment comprises of ratherspacious tunnels and multiple intersections hence easier tofind efficient exploration paths. Nevertheless, the MBPlannerachieves the highest exploration rate on average thanks toits fast and smooth planning trajectories. Regarding theright mine, the MBPlanner provides a superior performancecompared to the other two methods. NBVP gets trappedat the first narrow passage since its random tree is toosparse as it spreads over the whole explored map. TheFrontierPlanner fails to provide comparable results becauseit spends unnecessary effort trying to reach inaccessiblefrontiers detected inside tight spaces.V. EXPERIMENTAL EVALUATIONAlongside the main experimental studies presented in [22]that involved a collision-tolerant flying robot, in this ap-pendix we present additional experiments that among othersfocus on the global planning behavior. In these experimentswe utilized a different aerial robotic platform to evaluate thecombination of the local and global planning stages of theproposed exploration architecture. The utilized autonomousflying robot is developed around a DJI Matrice M100 andintegrates a multi-modal sensor fusion solution combinedwith loosely-coupled LiDAR Odometry And Mapping, aswell as visual-inertial localization. The system relies onModel Predictive Control (MPC) for its automated operation.The proposed planner subscribes to the data provided by thelocalization and mapping solution and provides referencesto the onboard MPC. Details for the overall system solutioncan be found in [2, 3, 26, 27]. The depth sensor integratedon the platform is a Velodyne PuckLITE which provides ahorizontal and vertical field of view of F H = 360 ◦ , F V =30 ◦ . It also has a maximum range of m, while a mapupdate takes place only for the first m of ranging. Based onthe robot size and safety considerations, the bounding box forthis test was set to length × width × height = . × . × . m.The system is first deployed and tested inside corridors of theApplied Research Facility (ARF) building at the Universityof Nevada, Reno. This environment emulates an undergroundsetting with narrow passages leading to dead-end situationsand also a branching point which overall requires the globalplanning behaviour. Another field experiment is conductedinside one of the Truckee River abandoned railroad tunnels ig. 3. Simulation results for autonomous exploration inside a room-and-pillar mine. The robot is tasked to explore either the left or the rightsection of the environment. The left column shows exploration maps andindicative planning examples from each planner in the left mine. In general,they all demonstrate promising performance in this environment, mainlydue to its wide corridors and presence of multiple nearby intersections forexploration at any location. As shown, the mission with MBPlanner wasalmost complete, while NBVP tends to provide solutions biased towardsspacious corridors since its samples are spanned over the whole map.The FrontierPlanner achieves adequate performance but with the lowestexploration rate because it does not account for the range sensor modelas MBPlanner and NBVP do. In the right column of the figure, indica-tive exploration results from each planner are depicted. The MBPlanneroutperforms the other two methods. The FrontierPlanner struggles withinaccessible frontiers detected inside narrow areas, while NBVP gets trappedin the first corridor since its random tree, expanding over the whole map,is not dense enough to pass through the first narrow passage. which were historically used to connect California withNevada and towards the East Coast of the U.S. by train.This specific tunnel system is relatively straight but involvessignificant height and interesting rock-wall geometry which,as it turned out, provided sufficient edges and broadly,geometric features to constraint the localization and mappingprocess.In the first test, the robot took-off inside one corridorof the ARF building ( m in width and . m in height)and progressively explored the left branch until reachingthe first dead-end. The global planner was then queried inorder to re-position the robot back to the main corridor tocontinue the exploration. The global path from the graph isfurther refined to provide a smoother trajectory respectingthe vehicle dynamics. Finally the homing procedure was Fig. 4. Comparison of exploration progress from the three consideredplanners (MBPlanner, NBVP, and FrontierPlanner) in a simulated room-and-pillar mine. The left and the right sub-figures show the changes inthe exploration volume during five 15min independent runs inside the leftand the right section of the mine respectively. The solid lines present theaverage over the whole five runs associated with shaded areas which are thelower and upper bound from those runs. Overall, MBPlanner outperformsthe other two methods and works reliably in both cases. NBVP is incapableof providing planning solutions for exploring narrow spaces due to itsfixed–global sampling space setting, thus leading to an insufficient numberof samples to find feasible paths through narrow corridors within reasonablecomputational bounds. Furthermore, the FrontierPlanner achieves slowerexploration rate and is sensitive to inaccessible frontiers that arise in tightspaces. triggered once the robot completely explored all corridorsin the defined bounds. The traveled distance for the wholemission is approximately m in total with the averagespeed being set to . m/s. As shown in Figure 5, the robotwas able to complete the exploration mission utilizing boththe local and global planner, and safely return to the initialstarting location.In the second experiment, inside the Truckee train tunnel,the robot is tasked to explore the tunnel with a speedsetpoint of . m/s and must return to the home locationautomatically given a limited minutes flight time. The robotwas deployed inside the tunnel, autonomously exploring themain tunnel using the local planner until its remaining timeapproaches the limit. At this point the global planner wasautomatically invoked to provide the shortest homing path.It is noted that, different to previous test environments, thistunnel is quite tall (approximately . m high) which requiresthe local planner to vary the robot’s height to map the wholeenvironment. The Figure 6 depicts the exploration process.The robot traveled a total of about m in minutes.The complete set of these field experiments, alongsidethose presented in [22], demonstrates the real-life potentialof the proposed planner to explore complex subterraneanenvironments using aerial robots. Especially when combinedwith a collision-tolerant design (see [22]) it enables the fastand agile exploration which in turn allows to map large-scale and narrow settings. The latter is particularly importantfor subterranean environments which may present severalchallenges to the perception system.VI. CONCLUSIONSIn this short appendix we outlined the architecture ofthe enhanced local-global search policy of the “Motionprimitivies-based exploration path planner” (MBPlanner)method which is now available open-source. A set of sim- ig. 5. Autonomous exploration inside corridors of the Applied ResearchFacility at the University of Nevada, Reno. The environment consists oftwo main narrow corridors and a three-way intersection. This environmentis suitable to test the local planning in narrow settings, as well as the globalre-positioning and auto-homing behavior. The robot was initialized insideone corridor, progressed through the first straight corridor then turned to theleft branch. It continued exploring the left corridor until reaching its endpoint (sub-figures 1-2). The global planner was then triggered to providea repositioning path back to the first corridor, followed by a refinementstep to smooth the path (sub-figure 3-4). Finally, the homing operation wasengaged after the robot finished the exploration (sub-figure 5). The travelleddistance is about m in total during the -minute flight. A video of thisexperimental can be found at https://youtu.be/_z2Sc8ANQa8?t=125 . ulation and experimental studies present the performanceof the strategy, its true D exploration capabilities andthe utilization of the global planning mode to re-positionthe robot towards previously identified frontiers of the ex-ploration space. Last but not least, autonomous homingoperation is also provided and ensures that the robot self-commands a homing trajectory when its remaining batterylife necessitates. The code related to this work is released asan open-source ROS package ( https://github.com/unr-arl/mbplanner_ros ).R
EFERENCES[1] B. Grocholsky, J. Keller, V. Kumar, and G. Pappas, “Cooperative airand ground surveillance,”
IEEE Robotics & Automation Magazine ,vol. 13, no. 3, pp. 16–25, 2006.[2] S. Khattak, C. Papachristos, and K. Alexis, “Keyframe-based directthermal–inertial odometry,” in
IEEE International Conference onRobotics and Automation (ICRA) , May 2019.[3] C. Papachristos, S. Khattak, F. Mascarich, and K. Alexis, “Au-tonomous navigation and mapping in underground mines using aerialrobots,” in , March 2019, p. toappear .[4] S. Arora and S. Scherer, “Randomized algorithm for informativepath planning with budget constraints,” in . IEEE, 2017, pp.4997–5004. Fig. 6. Autonomous exploration mission inside the Truckee RailroadTunnel, California. This abandoned train tunnel is mostly straight and hasdimensions of × × m. The robot, based on a Matrice M100 platform,was initialized inside the tunnel and commanded to explore the tunnel for minutes with a nominal speed of . m/s. The local planner providedsmooth paths for rapid exploration and the global planner triggered thehoming procedure at the end. In total, the robot was able to explore around m and came back safely to the initial starting location. A video of thisexperimental can be found at https://youtu.be/_z2Sc8ANQa8?t=236 .[5] C. Papachristos, S. Khattak and K. Alexis, “Autonomous explorationof visually-degraded environments using aerial robots,” in . IEEE,2017.[6] F. Mascarich, S. Khattak, C. Papachristos, and K. Alexis, “A multi-modal mapping unit for autonomous exploration and mapping ofunderground tunnels,” in IEEE Aerospace Conference (AeroConf) ,March 2018.[7] H. Balta, J. Bedkowski, S. Govindaraj, K. Majek, P. Musialik, D. Ser-rano, K. Alexis, R. Siegwart, and G. De Cubber, “Integrated datamanagement for a fleet of search-and-rescue robots,”
Journal of FieldRobotics , vol. 34, no. 3, pp. 539–582, 2017.[8] T. Tomic, K. Schmid, P. Lutz, A. Domel, M. Kassecker, E. Mair,I. L. Grixa, F. Ruess, M. Suppa, and D. Burschka, “Toward a fullyautonomous uav: Research platform for indoor and outdoor urbansearch and rescue,”
IEEE robotics & automation magazine , vol. 19,no. 3, pp. 46–56, 2012.[9] E. Galceran and M. Carreras, “A survey on coverage path planningfor robotics,”
Robotics and Autonomous Systems , vol. 61, no. 12, pp.1258–1276, 2013.[10] A. Bircher, M. Kamel, K. Alexis, M. Burri, P. Oettershagen, S. Omari,T. Mantel and R. Siegwart, “Three-dimensional coverage path planningvia viewpoint resampling and tour optimization for aerial robots,”
Autonomous Robots , pp. 1–25, 2015.[11] A. Bircher, K. Alexis, M. Burri, P. Oettershagen, S. Omari, T. Manteland R. Siegwart, “Structural inspection path planning via iterativeviewpoint resampling with application to aerial robotics,” in
IEEEInternational Conference on Robotics and Automation (ICRA) , May2015, pp. 6423–6430.[12] K. Alexis, C. Papachristos, R. Siegwart, and A. Tzes, “Uniformcoverage structural inspection path–planning for micro aerial vehicles,”in .IEEE, 2015, pp. 59–64.[13] C. Connolly et al. , “The determination of next best views,” in
IEEEInternational Conference on Robotics and Automation 1985 . IEEE,1985.[14] B. Yamauchi, “A frontier-based approach for autonomous exploration,”in
Proceedings 1997 IEEE International Symposium on ComputationalIntelligence in Robotics and Automation CIRA’97. ’Towards Newomputational Principles for Robotics and Automation’ , 1997, pp.146–151.[15] M. Popovic, G. Hitz, J. Nieto, I. Sa, R. Siegwart, and E. Galceran,“Online informative path planning for active classification using uavs,” arXiv preprint arXiv:1609.08446 , 2016.[16] M. Nieuwenhuisen and S. Behnke, “Search-based 3d planning andtrajectory optimization for safe micro aerial vehicle flight under sensorvisibility constraints,” arXiv:1903.05165 , 2019.[17] A. Bircher, M. Kamel, K. Alexis, H. Oleynikova and R. Siegwart,“Receding horizon ”next-best-view” planner for 3d exploration,” in
IEEE International Conference on Robotics and Automation (ICRA) ,May 2016. [Online]. Available: https://github.com/ethz-asl/nbvplanner[18] C. Papachristos, M. Kamel, M. Popovi´c, S. Khattak, A. Bircher,H. Oleynikova, T. Dang, F. Mascarich, K. Alexis, and R. Siegwart,“Autonomous exploration and inspection path planning for aerialrobots using the robot operating system,” in
Robot Operating System(ROS) . Springer, 2019, pp. 67–111.[19] T. Dang, C. Papachristos, and K. Alexis, “Visual saliency-awarereceding horizon autonomous exploration with application to aerialrobotics,” in
IEEE International Conference on Robotics and Automa-tion (ICRA) , May 2018.[20] C. Papachristos, S. Khattak, and K. Alexis, “Uncertainty–aware re-ceding horizon exploration and mapping using aerial robots,” in
IEEEInternational Conference on Robotics and Automation (ICRA) , May2017.[21] T. Dang, F. Mascarich, S. Khattak, C. Papachristos, and K. Alexis, “Graph-based path planning for autonomous robotic exploration insubterranean environments,” in
IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS) , November 2019.[22] M. R. Dharmadhikari, T. Dang, L. Solanka, J. B. Loje, D. H.Nguyen, N. V. Khedekar, and K. Alexis, “Motion primitives-basedpath planning for fast and agile exploration using aerial robots,” in
IEEE International Conference on Robotics and Automation (ICRA) ,May 2019.[23] T. Dang, M. Tranzatto, S. Khattak, F. Mascarich, K. Alexis, andM. Hutter, “Graph-based subterranean exploration path planning usingaerial and legged robots,”
Journal of Field Robotics , 2020.[24] F. Furrer, M. Burri, M. Achtelik, and R. Siegwart, “Rotors-a modulargazebo mav simulator framework,” in
Robot Operating System (ROS) .Springer, 2016, pp. 595–625.[25] S. Karaman and E. Frazzoli, “Incremental sampling-based algorithmsfor optimal motion planning,”
CoRR , vol. abs/1005.0416, 2010.[26] C. Papachristos, and K. Alexis, “Thermal-inertial localization forautonomous navigation of aerial robots through obscurants,” in .IEEE, 2018.[27] M. Kamel, T. Stastny, K. Alexis, and R. Siegwart, “Model predictivecontrol for trajectory tracking of unmanned aerial vehicles using ros,”