Trajectory Replanning for Quadrotors Using Kinodynamic Search and Elastic Optimization
TTrajectory Replanning for Quadrotors UsingKinodynamic Search and Elastic Optimization
Wenchao Ding, Wenliang Gao, Kaixuan Wang, and Shaojie Shen
Abstract — We focus on a replanning scenario for quadrotorswhere considering time efficiency, non-static initial state anddynamical feasibility is of great significance. We propose areal-time B-spline based kinodynamic (RBK) search algorithm,which transforms a position-only shortest path search (suchas A* and Dijkstra) into an efficient kinodynamic search, byexploring the properties of B-spline parameterization. The RBKsearch is greedy and produces a dynamically feasible time-parameterized trajectory efficiently, which facilitates non-staticinitial state of the quadrotor. To cope with the limitation ofthe greedy search and the discretization induced by a gridstructure, we adopt an elastic optimization (EO) approach as apost-optimization process, to refine the control point placementprovided by the RBK search. The EO approach finds theoptimal control point placement inside an expanded elastic tubewhich represents the free space, by solving a QuadraticallyConstrained Quadratic Programming (QCQP) problem. Wedesign a receding horizon replanner based on the local controlproperty of B-spline. A systematic comparison of our methodagainst two state-of-the-art methods is provided. We integrateour replanning system with a monocular vision-based quadrotorand validate our performance onboard.
I. I
NTRODUCTION
Micro aerial vehicles (MAVs), in particular quadrotors,have gained wide popularity in various inspection and explo-ration applications. To meet the need for fully autonomousnavigation in unexplored environments, a real-time local re-planner producing smooth, dynamically feasible trajectoriesis of great significance. Many existing planning methods [1][2] [3] [4] follow a path planning and path parameterizationtwo-step pipeline. The path planning part only producesunparameterized path, while the path parameterization partchooses a feasible dynamical profile for the path. The two-step pipeline is popular due to its efficiency, but is problem-atic in replanning scenario, since the path planning part isunaware of the vehicle’s non-static initial states. For instance,the geometrically shortest path may diverge from the initialvelocity direction, resulting in jerky trajectories or failure ofthe path parameterization.Considering the replanning for quadrotor which has non-trivial dynamics, it is highly desirable to use kinodynamicmotion planners, to facilitate non-static initial state andensure dynamical feasibility. Sampling-based methods suchas kinodynamic RRT* [6] are asymptotically optimal but
This work was supported by the Hong Kong PhD Fellowship Scheme,and the Joint PG Program under the HKUST-DJI Joint Innovation Labo-ratory. All authors are with the Department of Electronic and ComputerEngineering, Hong Kong University of Science and Technology, HongKong, China. [email protected], [email protected],[email protected], [email protected]
Fig. 1: Illustration of our experimental testbed which is equippeda monocular camera, an Intel i7 processor and a NVIDIA JetsonTX1. The localization module is based on our Monocular VisualInertial Navigation System (VINS-Mono) [5], and mapping moduleis based on monocular dense mapping and TSDF fusion. computationally expensive with an execution time of 10sto 100s. Allen et al. [7] proposed a real-time kinodynamicadaptation of FMT*, however, the computation time is stillaround half a second. Another issue of sampling-basedmethods is that the randomized behavior may result inunpredictable performance [3], especially when only limitedsampling is permitted. Search-based method is suitable forreplanning in the sense that its results are consistent givensimilar observations of the environment. To this end, Liu etal. [8] proposed a primitive-based resolution-complete (i.e.,optimal in the induced lattice graph) search method usinglinear quadratic minimum time control. However, the runningtime is sensitive to both the discretization level and the orderof control input. For the high-order control input (such asjerk-controlled) or large dynamic range which needs a higherdiscretization level, the running time is not stable and maybe around one or two seconds.In this paper, we explore a novel angle of kinodynamicsearch by searching for B-spline control point placement, toimprove the time efficiency. Different from many existingworks [9] [10] [11] which follow the two-step pipelineand use B-spline as the path parameterization method, weproduce time-parameterized trajectory in the search processdirectly. Our contribution is that we explore three prop-erties of B-spline to transform a low-complexity position-only search (such as A* and Dijkstra) into an efficientkinodynamic search. First, thanks to B-spline’s local control property as elaborated in Sec. III, the evaluation of B-splinecan be done locally, resulting in an efficient expansion ofcontrol points, without re-evaluation of whole trajectory.Second, based on the convex hull property of B-spline,the dynamical feasibility constraints can be rewritten in alinear form, avoiding the computation overhead brought by a r X i v : . [ c s . R O ] M a r easibility checking. Third, the control cost can be evaluatedin closed-form , without solving computationally expensivetwo-point boundary value problem BVP problem. The re-sulting real-time B-spline based kinodynamic (RBK) searchshares a similar structure and complexity to the position-onlysearch, but outputs dynamically feasible time-parameterizedtrajectory, which facilitates non-static initial state.The RBK search is essentially a greedy adaptation of afull-scale B-spline based kinodynamic search. The full-scalesearch is optimal, but scales poorly with respect to the splineorder and number of connections of the grid, as elaboratedin Sec. IV. As verified in Sec. VI-A, the solution cost (thecontrol cost and time cost) of the RBK seach is between thefull-scale kinodynamic search and the position-only shortestpath search, and the RBK search is at least two orders ofmagnitude faster than the full-scale search. To cope with thepotential sub-optimality and grid discretization in the RBKsearch, we propose an elastic optimization (EO) approach asa post-optimization process, which finds the optimal controlpoint placement inside the expanded elastic tube and ensuresdynamical feasibility, by solving a Quadratically ConstrainedQuadratic Programming (QCQP) problem. Thanks to theenforcement of dynamical feasibility during the RBK search,the initial trajectory fed to elastic optimization is alreadyfeasible, enhancing the robustness of the post-optimization.We complete the replanning system by further adoptinga receding horizon planning (RHP) framework [3] with aB-spline specification using its local control property. Asliding window optimization strategy [12] is also introducedto bound the complexity of EO approach. We compare ourreplanning system with two state-of-the-art replanners [8][13] in simulation, and validate our performance on a realmonocular vision-based quadrotor as shown in Fig. 1.II. S YSTEM O VERVIEW
The overview of our autonomous planning system isshown in Fig. 2. We call RBK search the system front-end , and EO approach back-end . The RBK search providesan initial placement of control point, which will be refinedduring the optimization process. The elastic optimizationapproach consists of two steps, namely, the elastic tubeexpansion and elastic optimization. There are two modesof our front-end, i.e., passive mode and active mode. Forthe passive mode, the front-end is activated by collisionchecking, while in the active mode, the front-end is notonly activated by collision checking, but also by a timerdepending on the B-spline knot separation. The active modeis used to reject the effects of outliers, when the mappingis noisy in some circumstances. Basically, we work on aseries of densely connected B-spline control points, whichare constantly modified by the RBK search to avoid collision,and refined by the EO approach.III. B-
SPLINE C URVE AND R EPLANNING
A. Local Control Property and Replanning
We use uniform B-spline as the trajectory parameterizationmethod. The evaluation of B-spline of degree k − with uni- Fig. 2: A diagram of our autonomous replanning system. form knot sequence { t , t , t , . . . , t n } of fixed time interval ∆ t can be evaluated using the following equation: c ( u ) = n (cid:88) i =0 v i N i,k ( u ) , (1)where u is the normalized parameter which can be computedaccording to u = ( t − t i ) / ∆ t , for t ∈ [ t i , t i +1 ] . v i ∈ R is thecontrol point at time t i and N i,k ( t ) is the blending function,which can be computed via the De Boor-Cox recursiveformula [14]. Following the practice of B-spline which calls k consecutive knots as a knot span , we call the correspondingstacked control points as a span . For instance, for the knotspan { t , t , t , . . . , t k − } , the corresponding span is definedby V := [ v v · · · v k − ] (cid:124) ∈ R k × .An important property of B-spline is the local control .Namely, a single span of a B-spline curve is controlled onlyby k control points, and any control point only affects k spans, as shown in Fig. 3 (a). Mathematically, for the i -thspan covering k knots from t i to t i + k − , the correspondingcurve and its l -th order derivative can be evaluated in closed-form as follows: d c i ( u ) d l u = 1(∆ t ) l d b (cid:124) d l u M k V i , (2)where b = (cid:2) u u · · · u k − (cid:3) (cid:124) ∈ R k denotes the basis vec-tor, M k = ( m i,j ) ∈ R k × k denotes the basis matrix, where m i,j = k − (cid:0) k − k − − i (cid:1) (cid:80) k − s = j ( − s − j (cid:0) ks − j (cid:1) ( k − s − k − l − i ,and V i = [ v i v i +1 · · · v i + k − ] (cid:124) ∈ R k × stacks the k controlpoints of the i -th span.We incorporate the local control property into both akinodynamic search process and a receding horizon plan-ning framework. The usage of local control property in thekinodynamic search is elaborated in Sec. IV and its usagein the receding horizon planning is shown in Fig. 3 (b). Thecontrol points are divided into four types, namely, executedcontrol points, committed control points, optimizing controlpoints (the control points inside the sliding window) andunoptimized control points. The control point is unchange-able once committed. A stopping policy will be activatedif a collision is detected for the committed trajectory. TheRBK search and EO approach only affect the optimizingand unoptimized control points. As a result, the replacementcaused by either the search or optimization will not causeany disturbance to the evaluation at the trajectory server. B. Convex Hull Property and Dynamical Feasibility
Another important property of B-spline is the convex hull property. Given Eq. 2, the dynamical feasibility constraints a)(b)
Fig. 3: (a) shows an example of cubic B-spline. { p , . . . , p } denotethe six control points, while { t , . . . , t } denote the correspondingknots. Adjusting p will only affect the trajectory corresponding tospan 4. (b) shows how the local control property can be applied tothe replanning system. can be expressed by maximum derivative (velocity, acceler-ation, jerk, etc.) bounds in terms of control point placement.After applying the convex hull property, different orderderivatives of the whole B-spline curve can be completelybounded using only linear constraints, as in the followingProp. 1. The limitation of Prop. 1 is its conservativeness,given that Prop. 1 is a sufficient but not necessary condition. Proposition 1.
Given uniform B-spline of order k − andtime step ∆ t , there exists a constant linear combination S = M − k C l M k / (∆ t ) l ∈ R k × k , such that | Sv Di | ≤ u max l,D k × isa sufficient condition for the derivative along coordinate D tobe thoroughly bounded for the whole span, i.e., (cid:16) dc ( u ) d l u (cid:17) D ≤ u max l,D , ∀ u ∈ [0 , , where v Di ∈ R k is the stacked positionvector of coordinate D ∈ { X, Y, Z } in i -th span, and C l ∈ R k × k is a constant mapping matrix of the l -th derivativesatisfying d b d l u = C l b . Proof.
Interested readers may refer to our report [15] fordetailed proof.As described by Mellinger [2], the control cost of quadro-tor trajectory can be expressed by the integral over squaredderivatives (such as fourth derivative snap), which can alsobe evaluated in closed form in the case of uniform B-spline.The total control cost E c of the i -th span can be expressedby a weighted sum of the integral over squared derivativesof different orders as follows: E ci = k − (cid:88) l =1 (cid:90) w l (cid:18) dc i ( u ) d l u (cid:19) du = k − (cid:88) l =1 w l V (cid:124) i M (cid:124) k Q l M k V i (3)where Q l = (cid:82) (cid:0) d b d l u (cid:1) (cid:0) d b d l u (cid:1) (cid:124) du/ (∆ t ) l − is the HessianMatrix of the l -th squared derivative, which is constantfor uniform B-spline, and w l is the corresponding weight. | · | means the element-wise absolute value of a vector. Without special mention, our replanning system uses quinticuniform B-spline ( k = 6 ) to ensure the continuity up to snapfor quadrotor systems.IV. B- SPLINE B ASED K INODYNAMIC S EARCH
A. Basics of B-spline Based Kinodynamic Search
In this section, we introduce the basics of B-spline basedkinodynamic search. A uniform distributed M -connect spa-tial grid with N cells is chosen as the graph structure G ,with cell centers as vertices V , and their connections asedges E , i.e., G := ( V , E ) . For instance, a 3-D grid witheach cell connected to its neighbors is 26-connect. Twospans V i and V j are said to be neighboring spans if andonly if the last k − control points of V i coincide withthe first k − control points of V j . We define the initialsystem state as a given span V init := [ v , v , . . . v k − ] (cid:124) containing the first k control points. In the same way, goalstate V goal can be defined. Note that the initial and goalstate we define are slightly different from those in traditionalkinodynamic planners [6], given the fact that V init and V goal actually represent two short trajectories. The B-spline basedkinodynamic search problem is finding a set of neighboringspans S := { V , . . . , V n − k +1 } that minimizes the followingcost function,min S J ( S ) = n − k +1 (cid:88) i =0 E ci + λ ( n + 1)∆ t (4)where λ ≥ is the weight of total time cost. The dynamicalfeasibility constraints can be checked in terms of span. The collision-free constraints are enforced by choosing controlpoint placement in unoccupied cells. Later we will discusshow to ensure the safety of resulting trajectory based on this.To solve the graph search problem optimally, we need a full-scale search in terms of B-spline spans. The complexityof the full-scale search depends on the number of all possibleB-spline span patterns. Typically, searching for k − -orderB-spline spans on an M -connected grid will induce an M -connected span graph G (cid:48) with a scaling factor O ( M k − ) ofthe number of vertices, if no pruning is applied. For instance,for 5-order B-spline search on a 26-connect 3-D grid, thevertex scaling factor is almost , which is unacceptable. B. Real-time B-spline Based Kinodynamic Search
Given the high complexity of the full-scale search, wepresent the RBK search algorithm, which transforms thelow-complexity position-only search into an efficient kino-dynamic search, by exploring three properties of B-spline,namely, local control property, convex hull property and closed-form evaluation of control cost. The three propertiesexactly correspond to the three core operations in the searchprocess, namely, span retrieving and expansion, dynamicalfeasibility checking and cost evaluation. The RBK algorithmshares a similar structure and complexity to position-onlysearch algorithms such as A* and Dijkstra, but obtainsthe ability of evaluating the control cost and ensuring thedynamical feasibility. The result of the RBK search isdynamically feasible time-parameterized trajectory instead lgorithm 1:
RBK ( V init , V goal , G ) Initializes: O = I = ∅ ; Add ( O , p new , Cost ( V init )) ; while size ( O )! = 0 do p cur ← PopMin ( O ) , Add ( I , p cur ) ; ( V cur , c cur ) ← RetrieveSpan ( p cur ) ; if NearEnd ( V cur , V goal ) then return success ; end for V nbr ∈ Neighbor ( V cur ) ∩ p nbr / ∈ I do if CheckDynamics ( V nbr ) then c (cid:48) = Cost ( V nbr ) + c cur ; if p nbr ∈ O then if c (cid:48) < RetrieveCost ( p nbr ) then Update ( p current , p nbr ) ; end else Add ( O ,p nbr ,c (cid:48) + HeuristicCost ( V nbr )) ; end end end end of unparameterized path, thus being different from position-only search. Denote open set and closed set as O and I , andcurrent grid node and neighboring grid node as p cur and p nbr . Span retrieving and expansion : the position-only searchalgorithms such as A* and Dijkstra implicitly maintain atree structure via the predecessor data structure. Instead ofretrieving one predecessor as in the position-only search, theRBK search retrieves k − predecessors. By stacking these k − predecessors with current control point, the current spanis formed. The retrieving process is implemented in function RetrieveSpan ( p cur ) in Alg. 1 and is shown in Fig. 4 (a).Thanks to the local control property, the expansion can bedone locally just like position-only search, since there is noneed to re-evaluate of the whole trajectory when expandingto new control points. Therefore, using local control property,the tree structure in A* and Dijkstra can be transformed intoa tree of spans, enabling B-spline based search. Dynamical feasibility checking : using the convex hull property, we derive a sufficient condition for dynamicalfeasibility as introduced in Sec. III, which is actually linear.By using this condition in function
CheckDynamics ( V nbr ) ,dynamical feasibility can be guaranteed without computationoverhead, which speeds up the kinodynamic search process. Cost evaluation : Traditional kinodynamic planners [6] relyon solving the two-step boundary value problem (BVP)to determine the cost of connecting two states, which iscomputationally expensive. By using B-spline parameteri-zation, the cost of connecting to new expanded span canbe evaluated using closed-form solution according to Eq. 3,which is implemented in function
Cost ( V nbr ) in Alg. 1. Thelimitation of our method is that the expansion of spans arerestricted by the resolution and connection of the grid. Forexample, for 26-connect 3-D grid, the connection to the nextcontrol point is restricted to the 26 connections, resulting in (a) (b) Fig. 4: Illustration of the RBK search. (a) shows the span expansionprocess (Sec.IV-B), and (b) shows the result of the RBK search andthe comparison with the position-only A* search. limited representations of B-spline trajectories.The trajectory of the RBK search can be guaranteed to be collision free if moderate obstacle inflation is done.The analysis of how much inflation is needed is shownin our report [15], which means that the RBK searchcan be used as a standalone component without any post-processing, when computation is limited. The terminationcondition is implemented in
NearEnd ( V cur , V goal ) in Alg. 1.In practice, we terminate the search when the first controlpoint of end span is reached and append the end span to it,since we observe that the part of dynamical profile can beautomatically smoothed by the back-end. To speed up thesearch process, we use HeuristicCost ( V nbr ) to focus onsearching in the most promising direction. In practice, oneadmissible heuristic can be λ · norm ( p nbr − p goal ) /v max , where p goal is the first control point of end span. The design of moreinformative heuristic function is left as a future work.As shown in Fig. 4 (b), based on the start span, theRBK search explores the low-cost region and generatesdynamically feasible time-parameterized trajectory, while A*finds a shortest but unparameterized path, which does notcontain any kinodynamic information. If we directly use theA* shortest path as B-spline control points placement, theresulting trajectory has large acceleration at the beginning,which is not good for replanning. As verified in Sec. VI-A,the RBK search generally finds better trajectories in termsof the control cost and time cost, compared to the position-only A*. Moreover, the dynamical feasibility is guaranteedfor the RBK search. As verified in Sec. VI and Sec. VII,Alg. 1 only needs around 20 milliseconds to reach a goodsolution. V. E LASTIC O PTIMIZATION
To compensate for the potential sub-optimality of the RBKsearch and the limitation induced by the discretization ofthe grid, we present the post-optimization process, i.e., theEO approach, to fully utilize free space and further reducethe trajectory cost. Since the initial trajectory provided bythe RBK search is already dynamically feasible and time-parameterized, the post-optimization is equipped with atleast one feasible solution, which makes the optimizationprocess robust. The EO approach can be divided into twocomponents, namely, an elastic tube expansion algorithm lgorithm 2: ( Q , R (cid:48) ) ← ElasticTube ( P , C ELAS ) Initializes: d min infl , d max infl , d thres . R = R (cid:48) = Q = ∅ ; for p i ∈ P do ( n i , r i ) ← NNSearch ( p i , C ELAS ) ; −→ f = ( p i − n i ) / norm ( p i − n i ) ; while d max infl − d min infl > d tolinfl do d ← (cid:0) d max infl + d min infl (cid:1) / , p i, infl ← p i + d · −→ f ; ( n (cid:48) i , r (cid:48) i ) ← NNSearch ( p i, infl , C ELAS ) ; if abs ( r (cid:48) i − d − r i ) > d thres then d max infl ← d ; else d min infl ← d ; end end Q ← Q ∪ p i, infl , R (cid:48) ← R (cid:48) ∪ r (cid:48) i ; end and an elastic optimization formulation. The elastic tubeexpansion algorithm generates a series of connected localmaximum volume “bubbles”, i.e., an elastic tube. The elasticoptimization is to stretch the trajectory (so-called “elasticband”) inside the tube, so that the objective is minimized. A. Elastic Tube Expansion
We denote the set of control point placement provide byAlg. 1 as P := { p , p , . . . , p m } as the initial placement,excluding the first k − control points of the start span,and the last k − control points of the end span. Asshown in Alg. 2, the elastic tube expansion algorithm canbe divided into two steps: first, we construct the initialtube, by doing radius search for the initial placement P andget the nearest obstacle position n i . Second, we push thecenter of the bubbles in the direction −→ f (away from thenearest obstacle), while satisfying the criterion that the newbubble contains the original bubble, as required by condition abs ( r (cid:48) i − d − r i ) ≤ d thres , as shown in Fig. 5 (a). The inflationprocess is implemented in a binary search manner to reducecalling NNSearch . Alg. 2 will finally find a series of localmaximum volume bubble centers Q := { q , q , . . . , q m } based on the initial tube P . As for the parameter settings, d max infl and d min infl are the maximum and minimum inflationdistance respectively; d thres is the threshold for checkingwhether the new bubble contains the original one, and shouldbe set to a small value, e.g., less than the map resolution;and d tolinfl is the binary search end condition, which can be setto the resolution of the map. The function NNSearch is thenearest neighborhood search which can be done efficientlyif a KD-tree is maintained. The actual run time of Alg. 2 isaround 2 millisecond, as verified in Section. VI.
B. Elastic Optimization Formulation
We are inspired by the elastic smoothing method for car-like robots in [16]. The key insight of [16] is the convexrepresentation of geometric smoothness and enforcementof speed feasibility. However, the formulation in [16] iscompletely based on car model. In contrast to [16] which
RBK Control PointsStart SpanEnd SpanRBK Traj.Inflate DirectionSupport Position (a)
RBK Control PointsEO Control PointsStart SpanEnd SpanRBK Traj.EO Traj. (b)
Fig. 5: Illustration of elastic optimization approach. (a) shows theelastic tube expansion process (Sec. V-A). (b) shows the elasticoptimization process (Sec. V-B). worked on a tube limited by the initial path, we use Alg. 2to generate a local maximum volume elastic tube to enhancethe optimization performance. Furthermore, we representsthe control cost and dynamical feasibility constraints inclosed-form using the high-order B-spline parameterizationfor controlling quadrotors.We denote { x , x , . . . , x m } as the control point place-ment (optimization variable). We denote the stacked spansas { V i | i ∈ {− k + 1 , − k + 2 , . . . m }} which also contain thehybrid spans which consist of optimization variable andfixed start span or end span control points. The optimizationproblem can be expressed as follows:min m (cid:88) i = − k +1 E ci s.t x = p , x m = p m (cid:107) x i − q m (cid:107) ≤ r (cid:48) i , ∀ i ∈ { , . . . , m − }| Sv Di | ≤ u max l,D k × , ∀ i, D, l ∈ { , . . . k − } (5)where the first constraint is the position constraint of thefirst and last control point to maintain the connectivity. Thesecond constraint is to restrict the control point inside thesafety space, which is quadratic. The third constraint is toensure the dynamical feasibility using the sufficient conditionin Prop. 1, which is linear. As a whole, the formulation is aQCQP. As verified in VI, the EO approach can be completedin around 30 milliseconds. C. Enforcing Safety Guarantee
One issue of the elastic optimization formulation is thatrestricting control points inside the bubbles is not a sufficientcondition for safety, since 1) bubbles are placed with finitedensity and 2) B-spline does not exactly pass the controlpoints. The issue can be resolved in two steps: first we showthat piece-wise linear segments connecting the control pointscan be completely contained in collision-free space using atwo-level obstacle inflation strategy; and second, the B-splinetrajectory can be pulled arbitrarily closed to the piece-wiselinear segments using an iterative process of adding controlpoints. The two-level obstacle inflation strategy is as follows:we model the robot as a ball with radius δ r , and we search inconfiguration space C RBK with a larger minimum clearance c RBK min , while we generate the elastic tube and optimize theontrol points in the configuration space C ELAS with a smallerminimum clearance c ELAS min as shown in Fig. 5. Based onthe given grid size of RBK search, the maximum distancebetween control points d max can be computed. By the two-level inflation, we could have c RBK min − c ELAS min ) > d max , whichis used to ensure the connectivity of the initial tube. Based onthis, a sufficient condition for the piece-wise linear segmentsto be completely contained inside the elastic tube is that c ELASmin − δ r > √ − d max 2 . Then, if the collision is caused bythe deviation from the piece-wise linear segments, controlpoints are added till the collision is resolved. Thanks to thelocal control property, this can be done with high efficiencywithout re-evaluation of the whole trajectory. In the case ofcollision, we can always guarantee the success of iterativeprocess by realizing that if the same k − control pointsare added to one control point, the robot will exactly passthrough that control point. The linear segment can be closelyfollowed by adding control points on both sides. VI. A
NALYSIS
We begin with an individual test for the RBK search, thenwe test the replanning system. A set of default parametersis used for all the simulation: 1) Maximum velocity andacceleration are set to . m/s and . m/s respectively.Accordingly, u max ,D = 2 . m/s (velocity bound for each axis)and u max ,D = 3 . m/s . 2)The B-spline order is set to , andthe time step ∆ t is set to . s . 3) The control cost in Eq.3 isset to minimizing snap (only w being non-zero). The weightof total time cost λ in Eq. 4 is set based on the criterion thatthe time cost and the control cost are at the same order foreach span. 4) For the replanning system, the RBK search isimplemented on a × × uniform grid, with an actualscale of m × m × m . The sliding window size is setto , and the local sensing range is set to m . A. Kinodynamic Search Performance
To verify the performance of the RBK search, we compareit with the full-scale B-spline based kinodynamic searchand position-only A* path search. Since the full-scale B-spline based kinodynamic search is of high complexity, thecomparisons are done on a relatively small grid of × .We can not directly compare our method with the position-only A* search according to the cost function defined inEq. 4, since the path returned by the position-only searchdoes not contain any kinodynamic information. Hence, weassume that the position-only search results are directlyparameterized using B-spline, to illustrate the performanceif a position-only search is used as the system front-end.As shown in Fig. 6 (a), if the position-only A* shortestpath is used as the control point placement, there are dynam-ically infeasible parts as marked in red. Our RBK search andthe full-scale B-spline based search both guarantee dynam-ical feasibility, and in this case our RBK method actually Interested readers may also refer to our report [15] for detailed proof. The strategy is to guarantee collision-free for the post-processing. (a)
Simulation Number O p ti m a lit y R a ti o RBK SearchAstar Search (b)
Fig. 6: Illustration of the RBK search performance and comparisons.In (a), the start span (in cyan) has a non-zero initial velocity pointingto the right. The end span (in cyan) is at the same grid representinga static state. The path of the position-only A* search (in yellow)and dynamically infeasible parts (in red) are shown. The results ofthe full-scale B-spline based search and the RBK search are markedin blue and green respectively. finds the optimal solution. We further conduct Monte-Carlotesting by randomly choosing the start state (random locationand random pattern of the span) and the obstacle location.As shown in Fig. 6 (b), we demonstrate the optimality ratiostatistics (i.e., total cost divided by the optimal cost givenby the full-scale search). The RBK search finds a lower costsolution than position-only A* search for all 12 rounds andreaches the optimal solution for five rounds. As for the timeefficiency, the average computing time for the position-onlyA*, the RBK and full-scale search are . s , . s and . s , respectively. Therefore, the RBK search is about threeorders of magnitude faster than the full-scale search evenon the small grid. Note that more efficient heuristics can bedeveloped to improve the RBK performance and characterizethe optimality gap, and this will be left as a future work. B. Run Time Analysis of Replanning System
In this section, we test our replanning system on twodifferent maps, including the random map and Perlin noisemap . The Perlin noise map is a complex 3-D world, asshown in Fig. 7(b). We evaluate the run time efficiency ofour replanning system and list the statistics of all componentsas shown in Table. I. The overall trajectory illustrating thewhole round trip is shown in Fig. 7. TABLE I: Run Time Analysis on Different Maps
Maps
Replans Time(s) RBKSearch
Opt. Time(s) TubeExpan. Traj.Opt. TotalOpt.
Random Map( 0.25 pillars/ m ) 76 AvgMaxStd As we can see from Tab. I, on the random map, the RBKmethod consumes an average computing time of . s witha standard deviation of . s . The elastic tube expansionmethod can be done in . s , which is much faster thantraditional free space segmentation methods such as IRIS[17], which may take up to . s to find a solution. The elasticoptimization can be done in . s . On the Perlin noise map, https://github.com/HKUST-Aerial-Robotics/mockamapa) Replan on the random map (b) Replan on the perlin noise map Fig. 7: Illustration of our replanning system on different maps. which contains unstructured 3D obstacles, our method has asimilar performance as on the random map, showing that ourmethod works well in complex 3-D environments.
C. System Comparison With State-of-the-Art Methods
In this section, we conduct a system comparison with twostate-of-the-art methods, including the Continuous Trajectory(CT) [13] method and Searched-based Motion Primitive(SMP) method by Liu et al. [8]. The SMP method [8]constructs motion primitives online based on linear quadraticminimum time control, and conducts heuristic-guided searchto generate dynamically feasible trajectories. We set upa challenging obstacle-cluttered 3-D complex simulationenvironment containing walls and 3-D steps, as shown inFig. 8. The replanning strategy is choosing a local target ona given straight-line guiding path. For CT method, the initialguiding path is parameterized using polynomial, followingtheir practice. The CT method tries to use gradient-basedtrajectory optimization to push a short local trajectory outof the Euclidean Signed Distance Field (ESDF) formed bythe obstacles. For SMP method, we use receding horizonplanning in their paper [8], namely, for every time slot,we plan for the next time slot while executing currentcommitted trajectory. The replanning strategy for our methodis the passive mode, as introduced in Sec.II. We evaluatethe replanning system from the trajectory statistics and timeefficiency perspectives (as shown in Tab. II, CT is excludedin the table due to its low success rate).As shown in Fig. 8 (a), the CT method uses the ESDFto push the initial trajectory out of the obstacles, but suffersfrom a low success rate due to local minimum issue. TheSMP method is not real-time when the full dynamic range isapplied, since a large dynamic range requires a higher levelof discretization of the control input, which will result in adramatically growing state lattice. As shown in Tab. II, theSMP under a full dynamic bound may need up to . s (with a . s standard deviation), which is unacceptablefor real-time replanning. If we limit the dynamic boundsto be conservative, the SMP method (so-called SMP Con-servative) is efficient but scarifies the maneuverability. Notethat only acceleration-controlled SMP can give a real-timeperformance ( Hz) in 3-D environments [8]. To obtain asmooth enough control input, SMP uses the unconstrainedQP formulation in [1] to reparameterize the trajectory, butthis post-processing has no safety or dynamical feasibility
TABLE II: Performance of Different Replanning Methods
Method
Trajectory Statistics Time Efficiency (s)Mean Max Ave Max StdVel. ( m/s ) Acc.( m/s )SMP 1.46 1.73 0.064 0.621 0.086SMP (Conservative) 1.20 1.56 0.010 0.080 0.012Our Method 1.17 2.69 0.035 0.064 0.012 guarantee.The RBK method can be done efficiently under fulldynamic bounds, since grid-based B-spline search actu-ally induces a discretization of the state space. The RBKsearch outputs a snap-continued trajectory, while the real-time SMP is only velocity-continued. The limitation of theRBK method is its greedy nature and limited representationof trajectories due to the discretization of the grid. Theadvantage of using RBK search is to obtain a dynamically-feasible time-parameterized trajectory efficiently. Thanks tothis property, the post-optimization process has a feasibleinitial solution and rarely fails. Moreover, the EO refinementprovides both safety and dynamical feasibility guarantee.VII. E XPERIMENTAL R ESULTS
For onboard testing, the parameters are as follows: the timestep ∆ t is set to . s ; the maximum velocity and maximumacceleration are set to . m/s and . m/s repectively; andthe local planning range is set to m × m × . m (thecorresponding gird size is × × ). A. Indoor Replanning Performance
As shown in Fig. 9, our replanning system works incomplex 3-D environment with only local map. The wholetrajectory length is . m and total trajectory execution timeis . s . The average velocity of the quadrotor is . m/s with a maximum velocity of . m/s . The maximum accel-eration of the trajectory is . m/s and the whole trajectoryis dynamically feasible. There are totally calls of RBKsearch (active mode) with average computation time . s ,since the grid size is smaller compared to that in simulation.There are calls of EO. The average computation timeof elastic tube expansion and elastic optimization is . s and . s , respectively. B. Outdoor Replanning Performance
As shown in Fig. 10, we demonstrate the outdoor ex-periments. For Fig. 10 (a), the trajectory length is . m and total execution time is . s . The average velocity ofthe quadrotor is . m/s . The maximum acceleration ofthe trajectory is . m/s and the whole trajectory is dy-namically feasible. There are totally calls of RBK search(passive mode) with average computation time . s . Theaverage computation time of elastic tube expansion andelastic optimization is . s and . s , respectively. ForFig. 10 (b), the performance is similar and the detailedstatistics are shown in the video attachment. a) CT Method (b) SMP (c) SMP (Conservative) (d) Our Method Fig. 8: For the CT method in (a), ESDF is shown in blue. The red circle in (a) marks the part that is either infeasible or jerky due to thelocal minimum issue. For SMP with a full dynamic bound in (b), the primitive duration is set to s and level of discretization is set to3. The red circle marks the part which does not satisfy the real-time requirement ( Hz). For SMP (Conservative) in (c), the maximumvelocity is set to m/s , the maximum acceleration is set m/s , and the level of discretization is set to 1. For our method in (d), weuse the default parameters in Sec. VI.Fig. 9: In (a), only local map is available for replanning. The controlpoints found by RBK (in blue), executed control points (in pink),committed control points (in green), and optimizing control points(in yellow) are marked. The whole trajectory is shown in (b) andthe trajectory following result is shown in green. (a) (b) Fig. 10: Illustration of the outdoor experiments. (a) shows a flightthrough a pavilion (part of the map on the top is cut for visualizationpurpose). (b) shows the flight avoiding trees.
VIII. C
ONCLUSION AND F UTURE W ORK
We focus on the replanning scenario for quadrotors.We present the RBK search method, which transforms theposition-only search (such as A* and Dijkstra) into a kin-odynamic search, by exploring the properties of B-spline.The RBK method facilitates the quadrotor’s non-static initialstate and produces dynamically feasible time-parameterizedtrajectory instead of unparameterized path. We present anEO approach as the post-optimization process to refinethe trajectory, while the safety and dynamical feasibilityare guaranteed. The dynamical feasibility is guaranteed forboth the front-end and back-end to ensure the robustnessof the optimization process. We design a receding horizonreplanner using sliding window optimization strategy andlocal control property of B-spline. System comparisons andonboard experiments are provided to validate the superiorperformance of our replanning system. Extensions to thiswork may include developing informative heuristics for theRBK search to enhance the performance. R
EFERENCES[1] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning foraggressive quadrotor flight in dense indoor environments,” in
Intl. J.Robot. Research . Springer, 2016, pp. 649–666.[2] D. Mellinger and V. Kumar, “Minimum snap trajectory generation andcontrol for quadrotors,” in
Proc. of the IEEE Intl. Conf. on Robot. andAutom. , 2011, pp. 2520–2525.[3] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J.Taylor, and V. Kumar, “Planning dynamically feasible trajectories forquadrotors using safe flight corridors in 3-d complex environments,”
IEEE Robotics and Automation Letters , vol. 2, 2017.[4] F. Gao, Y. Lin, and S. Shen, “Gradient-based online quadrotor safetrajectory planning in 3d complex environments,” in
Proc. of theIEEE/RSJ Intl. Conf. on Intell. Robots and Syst. , 2017.[5] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monoc-ular visual-inertial state estimator,” arXiv preprint arXiv:1708.03852 ,2017.[6] D. J. Webb and J. van den Berg, “Kinodynamic rrt*: Asymptoticallyoptimal motion planning for robots with linear dynamics,” in
Proc. ofthe IEEE Intl. Conf. on Robot. and Autom. , 2013, pp. 5054–5061.[7] R. E. Allen and M. Pavone, “A real-time framework for kinodynamicplanning with application to quadrotor obstacle avoidance,” Ph.D.dissertation, Stanford University, 2016.[8] S. Liu, N. Atanasov, K. Mohta, and V. Kumar, “Search-based motionplanning for quadrotors using linear quadratic minimum time control,”in
Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst. , 2017.[9] M. Elbanhawi, M. Simic, and R. N. Jazar, “Continuous path smoothingfor car-like robots using b-spline curves,”
Journal of Intelligent &Robotic Systems , vol. 80, no. 1, pp. 23–56, 2015.[10] E. Koyuncu and G. Inalhan, “A probabilistic b-spline motion planningalgorithm for unmanned helicopters flying in dense 3d environments,”in
Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst. , 2008,pp. 815–821.[11] T. Maekawa, T. Noda, S. Tamura, T. Ozaki, and K. ichiro Machida,“Curvature continuous path generation for autonomous vehicle usingb-spline curves,”
Computer-Aided Design , vol. 42, 2010.[12] V. Usenko, L. von Stumberg, A. Pangercic, and D. Cremers, “Real-time trajectory replanning for mavs using uniform b-splines and 3dcircular buffer,” arXiv preprint arXiv:1703.01416 , 2017.[13] H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, andE. Galceran, “Continuous-time trajectory optimization for online uavreplanning,” in
Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots andSyst. , 2016, pp. 5332–5339.[14] K. Qin, “General matrix representations for b-splines,”
The VisualComputer , vol. 16, no. 3, pp. 177–186, 2000.[15] W. Ding, W. Gao, K. Wang, and S. Shen, “Trajectory replanning forquadrotors using kinodynamic search and elastic optimization,” http://uav.ust.hk/icra2018wenchao.[16] Z. Zhu, E. Schmerling, and M. Pavone, “A convex optimizationapproach to smooth trajectories for motion planning with car-likerobots,” in
Proc. of the IEEE Control and Decision Conf. , 2015, pp.835–842.[17] R. Deits and R. Tedrake, “Efficient mixed-integer planning for uavsin cluttered environments,” in