Real-Time Long Range Trajectory Replanning for MAVs in the Presence of Dynamic Obstacles
Geesara Kulathunga, Roman Fedorenko, Sergey Kopylov, Alexandr Klimchik
RReal-Time Long Range Trajectory Replanning forMAVs in the Presence of Dynamic Obstacles
Geesara Kulathunga, Roman Fedorenko, Sergey Kopylov, Alexandr Klimchik
Center for Technologies in Robotics and Mechatronics Components, Innopolis University, Russia [email protected], [email protected], [email protected], [email protected]
Abstract —Real-time long-range local planning is a challengingtask, especially in the presence of dynamics obstacles. We proposea complete system which is capable of performing the localreplanning in real-time. Desired trajectory is needed in the systeminitialization phase; system starts initializing sub-components ofthe system including point cloud processor, trajectory estimatorand planner. Afterwards, the multi-rotary aerial vehicle startsmoving on the given trajectory. When it detects obstacles, itreplans the trajectory from the current pose to pre-defined dis-tance incorporating the desired trajectory. Point cloud processoris employed to identify the closest obstacles around the vehicle.For replanning, Rapidly-exploring Random Trees (RRT*) is usedwith two modifications which allow planning the trajectory inmilliseconds scales. Once we replanned the desired path, velocitycomponents(x,y and z) and yaw rate are calculated. Those valuesare sent to the controller at a constant frequency to maneuverthe vehicle autonomously. Finally, we have evaluated each of thecomponents separately and tested the complete system in thesimulated and real environments.
Index Terms —MAVs, planning, RRT*, Rtree, dynamic obsta-cle, polynomial-splines
I. I
NTRODUCTION
Multirotor Aerial Vehicles (MAVs) draw attention not onlyamong the researches but also from the general audiencedue to the emerging trustworthiness towards the real-worldapplicability in the recent past. Rescue operations in dangerousplaces where humans are not able to reach, surveillanceand delivering of goods are a few good examples of MAVsapplications. In all the scenarios, robust planning is the keycomponent to obtain a promising result in spite of the otherconstraints (e.g., state estimating, controlling, etc). On certainoccasions, MAV may fly into completely unknown zone dueto lack of understanding the environment. It could be adangerous where MAV cloud move into tree canopy in whichobstacles are not captured within sensors’ field of view (FoV).Hence, this should be avoided at all cost. Thus, along withglobal planning, task can not be completed. Applying localreplanning considering dynamic obstacles is the correct wayto approach this problem.At the present time, most of the commercial MAVs do nothave robust local replanning capabilities. Even it supports,capacities are at the primitive level. Local replanning whileavoiding obstacles is a must to have for safe navigation, inthe sense that understanding of the environment is essentialto have proper reasoning about obstacles. In the recent works,Octomap is used for building a profoundly accurate map of the (a) Experimenting proposed solution in the filed(b) Experimenting on our simulator. Green color ellipsoid depicts thesearch space whereas blue color sphere depicts the second search spacewhich is used to generate random point when MAV is close to obstacles.Light green color line and red line denote target trajectory and trajectoryto be completed respectively. Replanned trajectory is shown in yellowcolored line
Fig. 1: Experimental result on our simulator and fieldenvironment. On the contrary, high computational power andconsiderable memory resources are required to incrementallyupdate the Octomap. Moreover, extending the area of themap is quite challenging while keeping high level of accuracybecause of high demanding of computing resources.In the proposed solution, a set of consecutive point cloudsis incrementally concatenated instead of building a persistentmap. Afterwards, the point cloud is extracted within a pre-defined radius of the sphere relative to the current positionof the MAV. To identify obstacles within the extracted pointcould, we used the method that is proposed in [1]. Then, theinstant obstacles map is constructed as an Rtree [2] whichutilizes for planning. When obstacles are encountered insidepre-defined zone, path segment within this zone is reprojectedby replanner while considering the initial target trajectory.For planning, we modified the original RRT* algorithm in-troducing quadratic search space and incorporating the targettrajectory by which MAV is to manoeuvre. a r X i v : . [ c s . R O ] M a y ur Contributions
1) Modified original RRT* algorithm introducing quadraticsearch space. Moreover, the random sample selectionprocess of RRT* is improved by incorporating thetarget trajectory. These changes helped to gain real-timeperformance in a cluttered environment.2) Proposing a software framework that executes in parallelto achieve real-time performance and further reduce theexecution time by using code-level optimizationII. R
ELATED W ORK
In this section, we discuss robotics motion planning liter-ature with focus on methods of local replanning. We splitour discussion into path planning, trajectory smoothing anddetecting of regions of interest followed by environmentrepresentation from point cloud.
Robotic Motion Planning is about using prior informationto generate a set of control inputs to maneuver the robotfrom its initial position to the goal position complemented byhaving full awareness of the environmental conditions (i.e.,appropriate reactions when robot sees the obstacles). This isachieved in two stages: globally and locally. In global motionplanning trajectory is generated based on static obstacles.On the contrary, local motion planning is dynamic planningin which trajectory is constructed based on current sensorinformation about the environment.
Path Planning has two branches: graph and samplingbased. A* and Dijkstra’s are the most commonly used algo-rithms in graph-based search. A grid (discretization of contin-uous space) is constructed as the search space. Then, this gridis considered as a graph which is utilized for finding a path.On the other hand, sampling-based algorithms including RRT*(Randomly exploring Random Tree) [3], PRMs (ProbabilityRoad Maps) and EST (Expansive-Spaces Tree) [4] work oncontinuous space and does not claim the optimality whereasgraph-based algorithms do claim it. However, sampling-basedtechniques are computationally efficient which is the mainreason those are suited for working with high-dimensionalsearch spaces.
Trajectory generation and smoothing are closely relatedto each other. Trajectory generation can be classified intothree subcategories: path planning followed by smoothing,optimization-based approaches (e.g., [5], [6]) and motionprimitive based approaches. Usually, smoothing is done usingpolynomial-spline rather than cubic spline. Cubic spline makestrajectory over smoother which gives low accuracy than apolynomial spline. Polynomial spline always keeps the originalwaypoints after smoothing. On the other hand, the mainadvantage of motion primitive approaches such as ( [7]–[9])over the others is that its execution time varies on average infew milliseconds in a cluttered environment.
Detecting regions of interest from point cloud andrepresenting the environment are quite challenging tasks toperform in real-time. In this experiment, we are interested inextracting obstacles within a given sphere while neglecting allthe noise information. Most of the previously proposed works, point cloud is fed into one of the following data structures tobuild a map of the environment: voxel grid [10], octomap[11] and elevation map [12]. However, all of the precedingmethods are computationally expensive. On the other hand,F. Gao [13] utilizes globally-registered point cloud directlybypassing map building for local online planning where it firstidentifies the regions of interest from the point cloud and theconstruct instance map for further processing. Environmentrepresentation is mostly constructed as an occupancy map.Octomap [14] which uses hierarchical octree is one of theways of representing the search space. Octomap consists ofsmall grids that are called 3D voxel [15]. One of the benefitsof octomap is that its voxels can be purged when they havesame information. But it requires considerably high memoryresources for storing. To overcome this memory limitation,authors of [16] purposed voxel hashing which stores only thesign distance information.
Local Obstacle Avoidance is the most crucial part oflocal planning. Reactive avoidance and map-based avoidanceare the dominant methods. In reactive avoidance, planningrelies on only current sensor information without buildinga persistence map of the surrounding. Map-based avoidancedepends on a map which is constructed using sensor dataor prior knowledge of the environment. Both of these canbe fallen into local minima which essential to bypass. Onthe contrary, collision-free trajectory generation by buildingenvironment incrementally using octree is suggested in thiswork [17]. However, when the environment is cluttered, thepreceding techniques do not work properly. Thus, in contrast topreceding methods, to mitigate those problems, Oleynikova etal., [18] proposed a local exploration strategy based technique,to traverse in a cluttered environment.III. M
ETHODOLOGY
Long-range trajectory planning while considering dynamicobstacles is always difficult owing to various reasons includingerror checking between current pose and desired trajectory,instantaneous reaction to avoid the dynamic obstacles and re-plan trajectory that satisfies real-time constraints. We proposea complete system that addresses all the preceding problemsrobustly.
A. System Architecture
The system is designed as a multi-threaded application, inwhich five threads are utilized for achieving high-performancecomputation capability in real-time. The high-level view ofthe system is shown in Fig. 2. Point cloud processing threadprocesses raw point cloud from lidar and separate the regionsthat interest. Here, regions that interest indicate the surround-ing obstacles. The output of point cloud processing thread isstored in a shared circular buffer (
Obs map ), which size canbe configured. The current pose of the MAV is one of themost important pieces of information to avoid obstacles inreal-time and appropriately reproject the trajectory. Thus, theresponsibility of the odometry thread is to update the currentpose ( P current ) of MAV at 15Hz; P current is also a sharedig. 2: High-level architecture of the system, complementedwith all the shared variables which are denoted in bold font.Point cloud and odometry are taken as inputs whereas currentvelocity will be the output of the system, provided thatplanning thread is triggered when obstacles present close toMAV. Current velocity is reset to zero during the trajectoryreplanningvariable. Trajectory estimator and Planner are interconnectedwith each other closely. The trajectory estimator ensures toreset velocity to zero when the obstacles present within apredefined radius ( obs a void z one ), send a signal to the plannerto reproject trajectory ( T projected ) up to the obs a void z one and replace current trajectory ( T online ) with T projected . Thecontroller expects a minimum number of control messagesto operate properly. The 5th thread assures this requirement.It publishes current velocity and yaw rate at 15Hz. Detailsinformation can be found in following sections. B. Trajectory Estimation
Initially, it is required to provide the trajectory in theform of waypoints as input to the system. Afterwards, a newset of waypoints is generated such that maximum distancebetween two waypoints is less or equals to replanning dis .Thus, the preceding process implies the feasibility of thetarget trajectory which defines as the feasibility analysis in. 3.Lets define two successive waypoints p and p such that | p − p | > obs avoid dis . Hence, intermediate waypointsare generated in between them with coordinates according toEq. 1. p inter x = obs avoid dis · sin ( φ ) · cos ( θ ) + p x p inter y = obs avoid dis · sin ( φ ) · sin ( θ ) + p y p inter z = obs avoid dis · cos ( φ ) + p z θ = atan p y , p x ) , φ = atan (cid:113) p x + p y , p z ) p = p − p (1)Afterwards, generated new waypoints are smoothed out toenhance the consistency of continuous trajectory ( T target ).After experimenting on several approaches, normalized poly-nomial B-splines are chosen for trajectory smoothing. Thenormalized polynomial B-spline can be represented by thefollowing Cox-de Boor recursive formula [19], [20]. q j, ( t ) = t − t j t j +2 − t j q j, ( t ) + t j +3 − tt j +3 − t j +1 q j +1 , ( t ) q i, ( t ) = (cid:26) t ∈ [ t i , t i +1 )0 t (cid:54)(cid:51) [ t i , t i +1 ) (2)In general, this can be written in matrix form as follows: [ q i − , ( u ) q i − , ( u ) q i, ( u )] = [1 u u ] M(i) p i − p i − p i (3)where u = t − t i t i +1 − ti , u ∈ [0 , , p j | j = { i, i − , i − } are consecutive control points (waypoints). According to theproof given in [20], M(i) stands for the i -th basis matrix ofthe B-Spline matrix which can be derived as follows: ( t i +1 − t i )( t i +1 − t i − ) t i − t i − t i +1 − t i − − t i +1 − t i ) t i +1 − t i − t i +1 − t i ) t i +1 − t i − t i +1 − t i t i +1 − t i − − ( t i +1 − t i )( t i +1 + t i +2 − t i ) m where m = t i +1 − t i t i +2 − t i − . Once T target is constructed, succes-sive step is to split T target into T online and T rest . T online ispart of the trajectory within replanning dis from T target ’sstart waypoint. Rest of T target belongs to T rest . All thepreceding steps up to this happens only once during the systeminitialization stage. Rest of the process is executed in parallelas shown in Fig. 2. Insight of trajectory estimator is givenin Fig. 3. Updating T online and T rest happens if and onlyif distance between first and last waypoint of T online is lessthan replanning dis where distance is calculated summingup euclidean distances between consecutive waypoints in T online . Updating of T online happens by taking consecutivewaypoints from T rest such that total distance of T online isless than replanning dis and fetch them to T online . Thisprocess happens as a sliding window fashion until the sizeof the both trajectories T online and T rest is not equal to zero.Thereby MAV comes to its final destination. P next denotesthe first waypoint in T online . Then if the L2 norm between P current and P next is less than δ , remove P next from T online ( pop f ront ( T online ) ).In each iteration, neighbouring obstacles are scanned withina pre-defined radius. If obstacles are present, signal to theplanner is sent. Then re-planning (projecting) of the trajectorystarts from P current to end pose of T online . Finally, we replace T online with T projected . More descriptions of the planner’soperation are given in the next section.ig. 3: Trajectory estimation: trajectory ( T target ) to be followed is constructed from provided a sequence of waypoints in theinitialization stage; afterwards, T online and T rest are updated in parallel where T online referees to trajectory waypoints withina predefined distance ( replanning dis ) from the current pose ( P current ) of MAV; Rest of the T target belongs to T rest C. Improved RRT* Local Planner
We have made two significant modifications to the originalRRT* algorithm. These changes were made to reduce theexecution time of the algorithm for finding a path.Let’s define the problem statement adhere with [3]. Searchor configuration space is denoted with X = (0 , where X ∈ R . Regions that are occupied by the obstacles aredenoted with X obs . Thus, X \ X obs becomes an open set where cl ( X \ X obs ) can be defined as the traversable space ( X free ).Here, cl ( . ) is denotes the closure of set X \ X obs . X start and X goal are the start and target position for planning where X start ∈ X free and X goal ∈ X free . σ : [0 , → R denotesthe waypoints of the planned path, provided that σ ( τ ) ∈ X free for all τ ∈ [0 , ; This path is called as a obstacle freepath. We are interested in finding a feasible path over anoptimal path because the execution time of the planner isa critical factor where it should able to find a feasible pathin real-time. Thus, feasible path exists if collision free path( σ ( τ ) ) is satisfies these two conditions: σ (0) = X start and σ (1) ∈ cl ( X goal ) . In order to define the cost of the feasiblepath, it is necessary to understand how RRT* algorithmworks as given in Algorithm. 1, in which random sample x rand ∼ U ( X free ) is generated in each iteration, where U ( . ) stands for the uniform distribution. Afterwards, the parentvertex is selected by inspecting neighboring nodes (searchwithin the pre-defined area) with respect to x new . If new parentvertex can connect with current x start , it becomes new parent.If x new is a vertex and it is connected to x near , drop theexisting edge and replace it with x new . This process is calledrewiring. Feasible path is denoted as X start f ( x ) −→ X goal , where f ( x ) is the cost of the feasible path. Since RRT* is sampling-based, f ( x ) depends on the traversable search space X free . In thewhole, f ( . ) is a heuristic function which can be reformulatedas follows: f ( x ) = ˆ f ( x, X start ) + ˆ g ( x, X goal ) , x ∈ X free (4)where ˆ f and ˆ g both are admissible heuristic functions. Thus, f ( x ) does not try to overestimate the cost of the path. Whenthe search space ( X ) is bigger, it utilises a considerable amountof time to find a feasible path. We have noticed reducingthe search space will help to converge the planner faster.Therefore, we introduce a new search space X reduced whichis a subset of X free . X reduced is a polate ellipsoidal searchspace as shown in Fig 4 as similar to [21].To prove X reduced search space has higher convergence rateover X free , let’s define the probability of selecting a randomsample between X free and X reduced ( X reduced ⊂ X free ). P ( x ∈ X free ) (cid:54) P ( x ∈ X reduced ) = λ ( X reduced ) λ ( X free ) λ ( X reduced ) λ ( X free ) = 34 πd | X goal − X start | λ ( X free ) (5)where λ ( . ) denotes the volume of the search space. As givenin Eq. 5, λ ( X reduced ) λ ( X free ) depicts the selecting a sample from X reduced always has higher probability. Transverse diameterand conjugate diameter of the ellipsoid are to be estimatedwhen it is needed to replan the trajectory of MAV. Then,generating samples from X reduced is possible.Three-dimensional Gaussian bell can be represented as anellipsoid where size and orientation of ellipsoid are described lgorithm 1 The main steps of RRT* algorithm. Coloredlines with background highlighting reflect modifications of theoriginal RRT* procedure RRT* V ← X start ; E ← ∅ ; T raj ← Desired trajectory for i=1,...,n do x rand ← GetF reeSample i x nearest ← N earest ( G = ( V, E ) , x rand , T raj ); x new ← GetSteerP ose ( x nearest , x rand ); if ObstableF ree ( x nearest , x new ) then X near ← GetN earByV ertices ( x new ) V ← V ∪ { x near } ConnectShortestP ath ( x new , x near ) if IsEdge ( x new ) then Rewine ( x new , x near ) E ← E ∪ { ( x new ) } end if if CheckSolution () then return G = (V,E) end if end if end for return
G = (V,E) end procedure
Fig. 4: The heuristic sampling space X reduced is depicted withellipsoid in which transverse diameter is equal to X goal − X start and conjugate diameter is a configurable parameter (i.e,4m) on x and z directionby the covariance matrix Σ . Thus, defining the X reduced canbe seen as an analogy for defining three dimensional Gaussianbell, with R stands for rotation matrix and variances as adiagonal matrix diag ( σ xx , σ yy , σ zz ) . The relationship between R, diag ( σ xx , σ yy , σ zz ) and Σ is: Σ = R diag ( σ xx , σ yy , σ zz ) R t (6)where σ xx = | X goal − X start | and σ yy = σ zz = d . Therotation matrix R aligns z (0,0,1) to X goal − X start and can be calculated as follows: R = I + [ v ] x + [ v ] x (1 − p . z ) | v | p = X goal − X start | X goal − X start | , z = z | z | , v = p × z [ v ] x = − v z v y v z − v x − v y v x (7)where [ v ] x is the skew-symmetric matrix of v . Pose ofellipsoid ( X reduced ) is represented in a compact form Σ whichallows to generate random samples within the X reduced . InAlgorithm 2, it is given how random samples are generated inwhich (cid:12) stands for element-wise multiplication. Algorithm 2
Generating samples from X reduced procedure S AMPLE F REE2: covm ← Σ npts ← Number of random points (i.e, 500) ndims ← Dimension size of the space (i.e, 3) center ← Center position of the ellipsoid v, e ← eig ( covm ) for i=1,...,npts do unit balls i ←∼ N (0 , for j=1,...,ndims pt do pt ( i, j ) ←∼ N (0 , end for end for pt opt ← ( pt t (cid:12) for j=1,...,npts do f ac ( j ) ← Σ ndimsi =0 pt opt ( j, i ) end for f ac ← ( unit balls (cid:12) (1 /ndims )) (cid:12) sqrt ( fac (cid:48) ) pnts ← zeros ( npts, ndims ) d ← (cid:112) ( diag ( e )) for i=1,...,npts do pnts ( i, :) ← f ac ( i ) pt ( i, :) pnts ( i, :) ← ( pnts ( i, :) (cid:12) d (cid:48) ∗ v (cid:48) ) + center end for return pnts end procedure As we stated, when forming the problem statement,we are interested in feasible path planning ratherthan optimal path planning. This guarantees algorithmprobabilistically complete but it might not be asymptoticallyoptimal. If the planner probabilistically completes: lim n →∞ P ( V n ∩ X goal (cid:54) = ∅ ; σ (0) = X start , σ (1) ∈ cl ( X goal )) should be equal to one. To achieve real-time performance,the planner should be capable of generating a feasible pathwithin milliseconds, in that asymptotic optimality shouldbe guaranteed. Asymptotic optimality is defined as eachof the positions (waypoints) in path σ ( τ ) has clearance ζ to its closest obstacle. Hence, we further improved RRT*planner by proposing a technique to satisfy the asymptoticalptimality, complemented by having real-time constraints.Proposed method is given in Algorithm. 3, that alwayschecks the clearance ( ζ ) when choosing x nearest pointwhile incorporating the trajectory as shown in Fig. 5. ζ depicts the distance between x nearest and closed waypointon the trajectory. This is the second improvement that hasbeen introduced to original RRT* (Algorithm 1: line 6).An example of path generations from original RRT* andimproved RRT* is shown in Fig. 6. Algorithm 3
Selecting proper value for x nearest while satis-fying asymptotic optimality procedure N EAREST ( G = ( V, E ) , x rand , T raj ) nearest obs ← ClosestObstacle ( x rand ) nearest waypoint ← ClosestW ayP oint ( nearest obs ) center ← nearest waypoint npts ← max attempt ← radius ← . attempt ← while i < = max attempt do covm (0 .
0) = covm (1 ,
1) = covm (2 , ← radius random points ← SampleF ree () for j=1,...,npts do dis ← | random points i − nearest obs if dis > obstacle f ail saf e dis then if CollisionF ree ( random points i ) then return random points i end if end if end for attempt ← attempt + 1 radius ← radius ∗ end while return pnts end procedure Fig. 5: Selecting proper value for x nearest while consideringclosest point to the trajectory from obstacle where d denotesthe clearance ζ which is a configurable parameter. Selecting x nearest is given in Algorithm. 3IV. E XPERIMENTAL R ESULTS
In this section we present a complete validation of theproposed long range local replanner. First, we validate our Fig. 6: Path generated by original RRT* and correspondingB-spline trajectory is depicted in orange and green colorsrespectively, whereas paths that are denoted in blue color andback color correspond to path and smoothed path of improvedRRT*proposed instance map building with two other techniques:octomap [14] which is one of the most popular techniquesamong the researches and 3D Circular Ring Buffer [6]. Sec-ondly, performance of improved RRT*, original RRT*( [3])and A* are compared. Complete system is implemented inC++11 in that the sparse matrix library Eigen is used while en-abling GNU C++ compiler optimization level to -O2. Finally,complete system is evaluated in the simulated and the realenvironments. The simulation is done on a laptop equippedwith a 2.20 GHz Intel Core i5-5200U CPU and 8 GB RAM.The flight experiments are done on a DJI M600 quadrotor(Fig. 1a) equipped with an Intel NUC (dual-core CPU i5-4250Uat 1.30 GHz and 16GB RAM).
A. Instance Map Building
In our proposed approach, initial step is to filter out regionsthat interest from each Lidar point cloud. Afterwards, consec-utive regions of interest are merged. First step of trajectoryestimation thread is to extract regions with the obs avoid dis from current pose of MAV and push into a Rtree. That ishow instance map is constructed. We have used 10000 lidarpoint clouds and calculated average time of insertion. Resultis shown in Fig. 7. According to result, execution time for ourpropose solution is much less than for the OctoMap, whereas3D ring buffer performs slightly better than ours.
B. 3D Planner Performance
For evaluate performance of the proposed planner, wedeveloped 3D A* planner in addition to RRT* and improvedRRT*. All three algorithms utilize the same search space. Forperformance analyzes, we ran considered algorithms 100 timesfor a given search space. Likewise, we tested 10 different a) OctoMap [14](b) 3D Ring Buffer [6](c) Proposed instance map building approach
Fig. 7: Comparison of histograms of insertion time for oc-cupancy mapping. Obstacle avoidance zone (Lidar range) istaken as 5m in which resolution of the map is set to 20cmsearch spaces in which dimension on each direction (x,y andz) kept at 10m. Also, start and goal positions are kept atsame positions. Results are given in Table. I. In all the cases,improved RRT* takes minimum amount of time for findinga path. Usually RRT* is used for global planning. After themodifications we made, it is capable of finding path withina few milliseconds which lead us to employ this for localreplanning. To calculate cost of the smoothed path, we definea different kind of norm U path ( [22]). And its gradient canbe derived as given in Eqs. 8 and in Eqs. 9 in matrix form,assuming, path consists of n number of points. U path = 12 Σ ni =0 | q i +1 − q i | ∂U path ∂q i = 2 q i − q i +1 − q i − (8) ∂U path ∂q i = Aq = − ... − − ... − − ... ... ... ... ... . . . − q q q ... q n (9)Thus, cost of each path can be found by q t Aq . Since,desired trajectory( T online ) is known, we define the path cost as follows: path cost = q t AqT tonline AT online (10)Results are given in Table I.Fig. 8: Improved RRT* finds a path very closer to the giventrajectory(red dashed line). A* takes less time, cost of the pathmuch higher than even RRT*. This result corresponds to the3rd test result given in Table. ITABLE I: Search space set as 10m on each direction (x,y andz) with fix a set of obstacles (Fig. 8) in that voxel size is set to0.1m. Afterwards, improved RRT* (proposed), original RRT*and A* algorithms were executed 10 times. Execution timesand path costs are shown No. Time (ms) path cost
A* RRT* Ppsd A* RRT* Ppsd1 12.45 180.91 1.56 1.46 1.32 0.782 9.45 149.96 1.39 1.23 1.29 0.793 17.99 558.88 3.75 1.58 1.34 0.794 9.76 103.60 1.65 1.78 1.27 1.005 10.12 557.22 1.27 1.34 1.35 0.826 10.13 161.70 1.74 1.260 1.27 0.817 9.68 548.10 1.26 1.89 1.31 0.788 10.49 98.14 1.37 1.45 1.28 0.809 9.84 2300.89 1.60 1.28 1.28 0.7910 9.47 111.61 1.63 1.89 1.31 0.79
C. Evaluation of long range trajectory follower
Firstly, MAV is able to fly autonomously given any feasibletrajectory. Thus, We have evaluated our trajectory in a obstaclefree zone. Replanning is not required in obstacle free zone.Thus, MAV should fly on given trajectory. To estimate errorof trajectory following, MAV was tested on 5 different trajecto-ries. The cost of the path was calculated using Eqs.10. Averagevalue of cost is 0.45 ± a) One of the testing trajectories MAV should follow(b) The trajectory which MAV navigates autonomously is shown in purplecolour small ellipsoids where no obstacles are present Fig. 9: Example scenario for evaluation of trajectory follower
D. Testing the complete system
We have tested our complete system in simulated and realenvironments. Simulator is developed based on Unity 3d whichprovides simulating the flying environment, complementedwith 16 channels Lidar which gives the MAV surrounding asa point cloud. Two examples are shown in Fig. 10. For realword testing, we used DJI M600 hexacopter with VelodyneVLP16 Lidar, shown in Fig. 1a. Behaviour of drone when itdetects dynamic obstacle were experimented. (a)(b)
Fig. 10: Sample scenarios of replanning the trajectory in acluttered environment V. C
ONCLUSION
We have developed and tested the complete system forreal-time long-range local replanning presence of dynamicobstacles in real and simulated environments. Proposed im-provements for original RRT* helped us to achieve real-timeperformance. Still, there is room for improvement which wenoticed while testing. We are going to improve the proposedplanner by incorporating kinodynamic motion planning con-straints to increase the confident level of the planner. A
CKNOWLEDGMENT
The work presented in the paper has been supported byInnopolis University, Ministry of Education and NationalTechnological Initiative in the frame of creation Center forTechnologies in Robotics and Mechatronics Components (ISC0000000007518P240002)R
EFERENCES[1] G. Prathap, R. Fedorenko, and A. Klimchik, “Region of interest seg-mentation from lidar point cloud for multirotor aerial vehicles,” 2019.[2] A. Guttman, “R-trees: A dynamic index structure for spatial searching,”in
Proceedings of the 1984 ACM SIGMOD international conference onManagement of data , 1984, pp. 47–57.[3] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimalmotion planning,”
The international journal of robotics research , vol. 30,no. 7, pp. 846–894, 2011.[4] M. Akinc, K. E. Bekris, B. Y. Chen, A. M. Ladd, E. Plaku, and L. E.Kavraki, “Probabilistic roadmaps of trees for parallel computation ofmultiple query roadmaps,” in
Robotics Research. The Eleventh Interna-tional Symposium . Springer, 2005, pp. 80–89.[5] H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, and E. Gal-ceran, “Continuous-time trajectory optimization for online uav replan-ning,” in , Oct 2016, pp. 5332–5339.[6] V. Usenko, L. von Stumberg, A. Pangercic, and D. Cremers, “Real-time trajectory replanning for mavs using uniform b-splines and a3d circular buffer,” in . IEEE, 2017, pp. 215–222.[7] M. Pivtoraiko, D. Mellinger, and V. Kumar, “Incremental micro-uavmotion replanning for exploring unknown environments,” in . IEEE, 2013,pp. 2452–2458.[8] H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, and E. Gal-ceran, “Continuous-time trajectory optimization for online uav replan-ning,” in . IEEE, 2016, pp. 5332–5339.[9] A. J. Barry, P. R. Florence, and R. Tedrake, “High-speed autonomousobstacle avoidance with pushbroom stereo,”
Journal of Field Robotics ,vol. 35, no. 1, pp. 52–68, 2018.[10] A. Elfes, “Using occupancy grids for mobile robot perception andnavigation,”
Computer , vol. 22, no. 6, pp. 46–57, 1989.[11] K. M. Wurm, A. Hornung, M. Bennewitz, C. Stachniss, and W. Burgard,“Octomap: A probabilistic, flexible, and compact 3d map representationfor robotic systems,” in
Proc. of the ICRA 2010 workshop on bestpractice in 3D perception and modeling for mobile manipulation , vol. 2,2010.[12] S. Choi, J. Park, E. Lim, and W. Yu, “Global path planning on unevenelevation maps,” in
Ubiquitous Robots and Ambient Intelligence (URAI),2012 International Conference on. IEEE , 2012, pp. 49–54.[13] F. Gao and S. Shen, “Online quadrotor trajectory generation andautonomous navigation on point clouds,” in , Oct 2016,pp. 139–146.[14] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard,“Octomap: An efficient probabilistic 3d mapping framework based onoctrees,”
Autonomous robots , vol. 34, no. 3, pp. 189–206, 2013.[15] F. Steinbr¨ucker, J. Sturm, and D. Cremers, “Volumetric 3d mapping inreal-time on a cpu,” in . IEEE, 2014, pp. 2021–2028.[16] M. Nießner, M. Zollh¨ofer, S. Izadi, and M. Stamminger, “Real-time3d reconstruction at scale using voxel hashing,”
ACM Transactions onGraphics (ToG) , vol. 32, no. 6, p. 169, 2013.[17] J. Chen, T. Liu, and S. Shen, “Online generation of collision-freetrajectories for quadrotor flight in unknown cluttered environments,”in . IEEE, 2016, pp. 1476–1483.[18] H. Oleynikova, Z. Taylor, R. Siegwart, and J. Nieto, “Safe local explo-ration for replanning in cluttered unknown environments for microaerialvehicles,”
IEEE Robotics and Automation Letters , vol. 3, no. 3, pp.1474–1481, 2018.19] C. De Boor, “On calculating with b-splines,”
Journal of Approximationtheory , vol. 6, no. 1, pp. 50–62, 1972.[20] K. Qin, “General matrix representations for b-splines,”
The VisualComputer , vol. 16, no. 3, pp. 177–186, 2000.[21] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Informed rrt*:Optimal sampling-based path planning focused via direct sampling ofan admissible ellipsoidal heuristic,” in . IEEE, 2014, pp. 2997–3004.[22] K. J. C. S. Hesam H, Eli Wu,