Path Planning Followed by Kinodynamic Smoothing for Multirotor Aerial Vehicles (MAVs)

Geesara Kulathunga, Dmitry Devitt, Roman Fedorenko, Sergei Savin and Alexandr Klimchik Affiliation: Center for Technologies in Robotics and Mechatronics Components, Innopolis University, Russia
ggeesara@gmail.com, d.dmitry@innopolis.ru, r.fedorenko@innopolis.ru, s.savin@innopolis.ru, a.klimchik@innopolis.ru

Abstract

We explore path planning followed by kinodynamic smoothing while ensuring the vehicle dynamics feasibility for MAVs. We have chosen a geometrically based motion planning technique “RRT*” for this purpose. In the proposed technique, we modified original RRT* introducing an adaptive search space and a steering function which help to increase the consistency of the planner. Moreover, we propose multiple RRT* which generates a set of desired paths, provided that the optimal path is selected among them. Then, apply kinodynamic smoothing, which will result in dynamically feasible as well as obstacle-free path. Thereafter, a b spline-based trajectory is generated to maneuver vehicle autonomously in unknown environments. Finally, we have tested the proposed technique in various simulated environments.

Index Terms:

RRT*, iLQR, B-spline, OctoMap, Ellipsoidal search space

I Introduction

With the recent research advances in microcontroller technology and sensors capabilities, a new era has begun for MAVs. MAVs have been engaging with the plenty of applications including delivery, farming and cinematography in the recent past. Motion planning is one of the challenging tasks in almost all preceding scenarios. Subsequently, geometric based motion planning is a well-matured technique although no differential constraints (i.e., vehicle dynamics) are considered. Conversely, kinodynamics motion planning is one of the ways to ensure vehicle dynamics feasibility and fulfilling all the given constraints. When considering real-time motion planning, geometric based path planning followed by path parameterization is a well-adapted technique. But in this approach, path parameterization may fail despite selecting geometrically shortest path or the optimal selection of path finding algorithm.

Refer to caption — (a) A sample environment that is used for testing the proposed replanner

In this paper, we explore kinodynamic smoothing followed by trajectory generation to keep the dynamic feasibility while reducing the execution time compared to kinodynamic motion planning. We selected a sampling-based technique, RRT* [1] comes under geometric based planning that guarantees asymptotic optimality. On the other hand, RRT* may experience unpredictable performance issues due to its randomize behaviour. Thus, there is no guarantee of the computational cost. In the proposed solution, we have modified original RRT* [1] introducing an adaptive search space and a steering function which help to increase consistency of the planner. Besides, we propose multiple RRT* which generate a set of desired paths in which optimal path is chosen. Then apply kinodynamic smoothing, which will result in dynamically feasible as well as obstacle-free path. Thereafter, b-spline based trajectory is generated to maneuver the vehicle autonomously.

Our Contributions:

1.

We have modified original RRT* introducing an adaptive search space and a steering function which help to increase the consistency of the planner
2.

Proposing horizon based multiple RRT*. In each of the RRT* instances, it will find the optimal goal location and terminate at newly generated goal pose if the path length exceeds the prediction horizon
3.

Applying iLQR (Iterative Linear Quadratic Regulator) to smooth the optimal obstacle-free path which ensures the feasibility of the dynamic of the vehicle

II Related Work

Reasoning the environment in real-time is important for collision-free planning. Thus, keeping track of the environment (map) close by the current pose of MAV with a predefined perimeter is necessary under which map should be updated in an incremental fashion. Hornung. et al. [2, 3] proposed Octomap, which uses probabilistic occupancy estimation to construct the map which uses octree for storing the information. In the process of map building, random measurements (e.g., reflections, dynamics obstacles) should consider. Hence, environment representation with Octomap is concise and accurate enough for planning. Subsequently, voxel hashing [4] is another technique which uses Truncated Sign Distance Fields (TSDFs) [5] for reconstructing the environment.

Path planning can be classified into several levels [6]. Deterministic and probabilistic is one of the ways to classify them. For a given search space, the same result is expected for deterministic approaches, i.e., A* and Dijkstra. Deterministic approaches have several drawbacks including not effectively plan the path satisfying real-time constraints and increase the complexity when the search space dimension is higher, i.e., 3D. In contrast to deterministic approaches, probabilistic based approaches (e.g., Probabilistic Road Mapping [7], Randomly exploring Random Trees [8]) overcome those constraints despite adding more computation footprint. RRT* (Rapidly-exploring Random Tree) [1] is one of the well-adapted path planning techniques which can categorize as a sampling-based technique.

Most of the trajectory planning algorithms perform path planning followed by feasible trajectory generations as a two-step pipeline; This may be problematic when it is needed replanning in which path planner unaware of the vehicle’s dynamics. To address this problem, kinodynamic based motion planning [9] is highly desirable, which ensures the dynamic feasibility. Deterministic path planning algorithms such as A* as well as probabilistic path planning techniques (e.g., RRT*, BIT* [10], FMT* [11], etc) can be modified adding kinodynamic capabilities. However, real-time kinodynamic motion planning has been seen as an open problem yet due to high computational cost.

To generate optimal control for a given system, which is utilized in kinodynamic planning, there are various techniques have been proposed. Linear Quadratic Regulator (LQR) was suggested by Glassman and Tedrake [12] for kinodynamic RRT planner for generating optimal control inputs, provided system dynamics. Alejandro Perez, Robert Platt Jr [13] extended preceding idea for RRT* while linearizing the non-linear system dynamics at each newly sample point. Moreover, they use infinite horizon LQR policy to obtain optimal control inputs. Instead of using infinite horizon, Jur and Berg [14] propose finite horizon iterated LQR and extended LQR smoothing techniques considering non-linear dynamics and non-quadratic cost for optimal kinodynamic motion planning.

Fig. 2: Initially, the vehicle stays in the wait state until the goal pose is given. If the goal pose within the map and no obstacles are on the goal pose, change its state into Gen which will find the optimal trajectory in between current pose and the goal pose. Afterwards, the state will change into the Exec state until it reaches the goal pose. If there are obstacles close by vehicle, it retries to generate a new trajectory. This process continues until it navigates to the goal pose. Besides, Exec state can be changed into Wait state, if there are some sensor malfunctions which cause the sensor streams interruption. PD (P-proportional, D-derivative) regulator is taking actual and desired odometry and calculate the estimated velocity and yaw angle for controlling the vehicle

Path planning results in a sequence of waypoints which are connected through a set of straight lines and sharpen turns. Thus, a path may not be desirable for navigation due to three constraints: geometric continuity, safety and feasibility of the vehicle dynamics. To fulfil these constraints, the path is to be smoothed ensuring preceding constraints. Optimization-based approaches can satisfy all three preceding constraints. Work in [15], proposes a convex elastic smoothing (CES) algorithm, for trajectory smoothing as a convex optimization problem. Timed Elastic Band (TEB Planner) [16] is a kinodynamic planner which locally optimizes the trajectory while considering provided constraints. Zhou et al. [17] propose a kinodynamic local planner, based on A* which works quite aggressively.

B-spline [18], minimum-snap [19] and its variants are widely used for trajectory generation in the recent past. B-spline [20, 21] are extensively used for trajectory generation due to several reasons. Clear geometrical meaning is one of the main reasons which highly adapted in trajectory generation in 3D space. B-splines, much simpler from the computational point of view because local changes in the trajectory can be done quickly and easily without recomputing the entire trajectory [22].

III Methodology

Pictorial visualization of the proposed framework is depicted in Fig. 2 whereas the workflow is shown in Fig. 3. The following sections explain in detail how each of the components contributes to acceptable system functioning.

III-A Environment Representation

Since we use a depth camera for reasoning the environment, constructing incremental map building is necessary because the camera has only (85.2’x58’x94’) field of view (FoV) which is not enough for planning. Thus, initially, we feed the camera depth map into Octomap server. Octomap server constructs the map of the environment incrementally. Thereafter, point cloud around the current pose of the vehicle is extracted. RTree [23] is constructed from an extracted point cloud which represents the instance map of the environment.

III-B Adaptive search space

RRT* is a well-known technique for path planning in high dimensional spaces, i.e. 3D in which search space defines the whole map of the environment in the default setting. We have made several changes to reduce the execution time and increase consistency. Thus, it is good for global path planning not for local planning specially replanning. On the contrary, in this study, we use RRT* for local replanning. Hence, ensuring consistency of consecutive paths is required. One way to improve consistency is by generating proper random samples. Subsequently, defining the optimal local search space which is closer to the current pose of the vehicle. Thus, we proposed a deterministic way of generating search space which eventually helps to improve the consistency of the planner significantly.

Search space is being constructed as an ellipsoidal search space (spherical or oblate spheroid or prolate spheroid) in which principal axes are defined by start and goal pose of the trajectory. In general, random samples can be generated within constructed ellipsoid, but we proposed a deterministic way of generating sample points (Algorithm 1) as similar to [24] while considering constraints of the traversable space. Besides, this helps to reduce the execution time. Since search space is being changed according to the traversable space around the current pose of the vehicle, we call it as an adaptive search space. RRT* planner picks points randomly from the deterministic search space.

Adaptive search space is defined as

(x-c_{x})^{2}/(r_{x})^{2}+(y-c_{y})^{2}/(r_{y})^{2}+(z-c_{z})^{2}/(r_{z})^{2}=1

(1)

where c is the middle pose in between start and goal pose of the trajectory and $\mathbf{r}=<r_{x},r_{y},r_{z}>$ is defined as follows:

\begin{split}\mathbf{r}=\mathbf{x_{goal}}-\mathbf{x_{start}}\end{split}

(2)

where $\mathbf{r}_{x}=max(4.0,\mathbf{r}_{x}),\;\mathbf{r}_{y}=max(4.0,\mathbf{r}_{y})\;$ and $\mathbf{r}_{z}=max(4.0,\mathbf{r}_{z})$ . The rotation matrix R is calculated between $\mathbf{z}$ (0,0,1) and $\mathbf{r}$ as given here [25].

Algorithm 1 Generating points over the interior of the search space

1 input data :

n

points to be generated,

r

principal semi-axes, R rotation matrix

c

center position

cols=0,\;n_{p}=0,\;r_{min}=min(r),\;M=zero(3,n)

3 if $r_{min}==r_{x}$ then

h=2*\frac{r_{x}}{2n+1}

n_{i}=n

n_{j}=\frac{r_{y}}{r_{x}}*n

n_{k}=\frac{r_{z}}{r_{x}}*n

5 end if

6 if $r_{min}==r_{y}$ then

h=2*\frac{r_{y}}{2n+1}

n_{j}=n

n_{i}=\frac{r_{x}}{r_{y}}*n

n_{k}=\frac{r_{z}}{r_{x}}*n

8 end if

9 else

h=2*\frac{r_{z}}{2n+1}

n_{k}=n

n_{i}=\frac{r_{x}}{r_{z}}*n

n_{j}=\frac{r_{y}}{r_{z}}*n

11 end if

12 for $k={0,...,n_{k}}$ do

z=c_{z}+k*h

14 for $j={0,...,n_{j}}$ do

y=c_{y}+j*h

16 for $i={0,...,n_{i}}$ do

x=c_{x}+i*h

M(0)=<x,y,z>

19 for $l={0,1,2,...,7}$ do

20 if $l==0||l==1||l==4$ then

n_{p}=0

22 end if

23 if $l\leq 0$ then

M(cols+l)=<2c_{x}-M(n_{p})_{x},M(n_{p})_{y},M(n_{p})_{z}>*R

25 end if

26 if $1\leq l\leq 2$ then

M(cols+l)=<M(n_{p})_{x},2c_{y}-M(n_{p})_{y},M(n_{p})_{z}>*R

28 end if

29 if $l\geq 3$ then

M(cols+l)=<M(n_{p})_{x},M(n_{p})_{y},2c_{z}-M(n_{p})_{z}>*R

31 end if

n_{p}++

cols+=8

34 end for

35 end for

36 end for

37 end for

III-C Multiple RRT*

To further improve the consistency, we use multiple RRT* instances using a thread pool which utilizes multi-cores/multi-processors will result in generating N number of paths. N is a configurable parameter. It is better to select N as the number of cores in your embedded computer because the thread pool is created in the phase of algorithm initialization with numbers of threads which equals to the number of system cores, which helps to improve performance. Initially, select the path that has the lowest cost as the optimal path. Cost is given by:

Cost=\left\|\left(\bf P_{\rm M}-\bf P_{\rm{goal}}\rm\right)\right\|+\sum_{m=1}^{M-1}\left\|\left(\bf P_{\rm m}-\bf P_{\rm{m+1}}\rm\right)\right\|

(3)

where $\bf P_{\rm{m}}$ depicts the mth waypoint of the selected path in which it consists of M number of waypoints. Always, it is not correct to consider the lowest cost belongs to the optimal path. We have to check the safely of the path. If the chosen path closer to obstacles, next time it will pick the path which has the second-lowest-cost. If the environment is cluttered, the optimal selection will go up to the highest cost as well. Once an optimal path is selected and start moving on the trajectory, next time, it will start from the lowest cost. If the distance between start and goal pose higher than a predefined value (planning horizon), RRT* will return path up to the planning horizon; This is the procedure for selecting an optimal path from generated paths.

III-D Path smoothing via iLQR

An optimal path which chooses from the preceding step is to be smoothed while considering obstacles around the vehicle. The optimal path consists of a set of waypoints which connect start and goal pose. Next step is to take consecutive three waypoints and get midpoints of first and second waypoints and second and third waypoints and apply iLQR between those two midpoints. iLQR is solved as a finite horizon (N steps) optimization problem. N, the number of steps at max is required to emulate vehicle dynamics.

We use the same quadcopter model as given in [26]. Let the system state space be $x=[p^{T}v^{T}r^{T}w^{T}]$ where p,v,r and w stand for position (m), velocity (m/s), orientation about axis $\mathbf{r}$ by angle $|r|$ (rad) and angular velocity (rad/s) respectively. Size of the state space ( $x$ ) equals to 12 (n) and system has 4(m) control inputs : $u_{1},u_{2},u_{3}$ and $u_{4}$ . Vehicle continuous-time dynamics $\dot{x}=f(x,u)$ is given as follows:

\begin{split}&\dot{p}=v\\ &\dot{v}=-ge_{3}+((u_{1}-u_{2}+u_{3}+u_{4})exp([r])*e_{3}-k_{v}v)/m\\ &\dot{r}=w+\frac{1}{2}[r]w+\frac{(1-\frac{|r|}{2tan(\frac{1}{2}|r|)})[r]^{2}w}{|r|^{2}}\\ &\dot{w}=J^{-1}(\rho(u_{2}-u_{4})e_{1}+\rho(u_{3}-u_{1})e_{2}\\ &\;\;\;\;\;\;\;+k_{m}(u_{1}-u_{2}+u_{3}-u_{4})e_{3}-[w])Jw)\end{split}

(4)

where $e_{i};i=1,2,3$ standard the basis, g stands for gravity, $k_{v}$ and $k_{m}$ are constants, m, J, $\rho$ are mass (kg), moment of inertia (kg $m^{2}$ ) and distance from center of vehicle to center of rotor (m) respectively. $[a]$ depicts the skew-symmetric matrix notation. Afterwards, it is needed to define an optimal control policy ( $\pi\in\mathbb{X}\rightarrow\mathbb{U}$ ) where $\mathbb{X}\subset\mathbb{R}^{n}$ and $\mathbb{U}\subset\mathbb{R}^{m}$ utilizes for generating a path in-between given two poses ensuring the vehicle dynamics and environmental constraints. Since system dynamics is non-linear at each considered discrete time instance, system dynamics is to be linearized as follows:

	$\displaystyle f(x_{k},u_{k})\approx\tilde{f}(x_{k},u_{k})=$		(5)
	$\displaystyle f(\bar{x_{k}},\bar{u_{k}})+\frac{\partial f}{\partial x}(\bar{x_{k}},\bar{u_{k}})(x_{k}-\bar{x_{k}})+\frac{\partial f}{\partial u}(\bar{x_{k}},\bar{u_{k}})(u_{k}-\bar{u_{k}})$		(5)

where $\bar{x_{k}}$ and $\bar{u_{k}}$ are the estimated values at kth step. $\bar{u_{k}}$ is estimated with the help of Riccati equation [27]. Then the optimal control inputs ( $u_{0}^{*},u_{1}^{*},...,u_{N-1}^{*}$ ) can be calculated as follows:


$\displaystyle\mathmakebox[\widthof{$\underset{\displaystyle u_{0},...,u_{N-1}}{\mathrm{subject~to}}$}][l]{\underset{\displaystyle u_{0},...,u_{N-1}}{\mathrm{minimize}}}\quad\sum_{k=0}^{N}{c(x_{k},u_{k})}$		(6a)
$\displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\phantom{u_{0},...,u_{N-1}}}{\mathrm{subject~to}}$}][c]{{\mathrm{subject~to}}}\quad$	$\displaystyle x_{1}=f(x_{0},u_{0}),$	(6b)
$\displaystyle x_{2}=f(x_{1},u_{1}),$		(6c)
$\displaystyle...,$		(6d)
$\displaystyle x_{N}=f(x_{N-1},u_{N-1})$		(6e)

where $x_{0}$ is a starting pose where the path is to be smoothed. Linearized cost $c(x_{k},u_{k})\approx\tilde{c}(\delta x_{k},\delta u_{k})$ can be defined as:

	$\displaystyle\tilde{c}(\delta x_{k},\delta u_{k})=c(\bar{x}_{k},\bar{u}_{k})+\Delta c(x_{k},u_{k})\begin{bmatrix}x_{k}-\bar{x_{k}}\\ u_{k}-\bar{u_{k}}\end{bmatrix}+$		(7)
	$\displaystyle\frac{1}{2}\begin{bmatrix}x_{k}-\bar{x_{k}}\\ u_{k}-\bar{u_{k}}\end{bmatrix}^{T}\Delta^{2}c(x_{k},u_{k})\begin{bmatrix}x_{k}-\bar{x_{k}}\\ u_{k}-\bar{u_{k}}\end{bmatrix}+q\Sigma_{i}exp(-d_{i}(x_{k}))$		(7)

where q is a scaling factor and $d_{i}(x_{k})$ is the sign distance between ith obstacle and considered pose $x_{k}$ . Once the optimal control inputs are obtained by applying 4th order Runge-Kutta integrator, projected poses can be calculated. Those poses represent the smoothed path from $x_{0}$ to intermediate goal pose.

IV Experimental Results

In this section, we present qualitative and quantitative validation of the proposed local replanner. First, we validate our proposed instance map building with two other techniques. Afterwards, the performance of the improved RRT* is compared with A* and original RRT*. Then, the complete system is evaluated in various simulated environments. The proposed planner is implemented in C++11 in that the sparse matrix library Eigen is used while enabling GNU C++ compiler optimization level to -O2. All the simulated experiments are conducted on a computer (Intel(R) Core(TM) i7-7500U CPU @ 2.70GHz CPU and 8 GB RAM).

RTree [23] based instance map is constructed when the depth map is available at 6Hz. We have adopted this approach to reduce the execution time, which utilizes for map building on the quadcopter. We have evaluated our instance map building approach with two other existing approaches as shown in Fig. 7. In the proposed approach, it is needed to keep two instance maps (current and previous). The previous map is utilized by the planner. Once the current map is built, the previous map replaces with the current map. Despite building an incremental map ( [18, 17]), the proposed approach will save computational cost and execution time while maintaining accuracy as similar to those incremental map building approaches.

We have compared the mean computation time of the improved RRT* with original RRT* and A*. All three algorithm are utilized the same search space that consists of 50 random obstacles within a cube of 20 m. We have generated 100 different search spaces and calculated mean computation time. Result is given in Table. I. In Fig. 8, it is shown an example scenario. We were able to reduce the computation time considerably. Besides, improved RRT* has a constrained search space, it helps to improve the consistency of the path finding as we explained in the section III-B. Also, we calculated the mean computation time of the main five operations that involve in the proposed planner as percentage values as shown in Fig. 9.

TABLE I: Mean computation time for path finding under which same search space utilized for 100 different trials

Original RRT*

Improved RRT*

Mean Computation

time (s)

1.9612

1.2512

0.9621

Finally, we have conducted a few experiments on different simulated environments. In Fig. 1(a), one of the selected simulated environment for testing the behaviour of the planner. When the distance between goal and start position less than the predefined horizon (Fig. 1(b)), it generates complete path or else it generates path up to the horizon as shown in Fig. 1(c).

V Acknowledgment

The work presented in the paper has been supported by Innopolis University, Ministry of Education and National Technological Initiative in the frame of creation Center for Technologies in Robotics and Mechatronics Components (ISC 0000000007518P240002)

VI Conclusion

We presented a framework for path planning which followed by kinodynamic smoothing while ensuring the vehicle dynamics feasibility for MAVs. We have chosen ”RRT*” for path planning in which several improvements have done to reduce the planning time, which helps to employ as a local planner. Kinodynamic smoothing ensures the dynamic feasibility and obstacle-free path. Finally, we have validated the proposed framework in various simulated environments to check the behaviour of the planner. Our validation result shows the performance of the planner. In future works, we are going to improve the efficiency of the path planner.

References

[1] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” The international journal of robotics research, vol. 30, no. 7, pp. 846–894, 2011.
[2] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard, “Octomap: An efficient probabilistic 3d mapping framework based on octrees,” Autonomous robots, vol. 34, no. 3, pp. 189–206, 2013.
[3] F. Steinbrücker, J. Sturm, and D. Cremers, “Volumetric 3d mapping in real-time on a cpu,” in 2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014, pp. 2021–2028.
[4] M. Nießner, M. Zollhöfer, S. Izadi, and M. Stamminger, “Real-time 3d reconstruction at scale using voxel hashing,” ACM Transactions on Graphics (ToG), vol. 32, no. 6, pp. 1–11, 2013.
[5] H. Oleynikova, Z. Taylor, M. Fehr, J. Nieto, and R. Siegwart, “Voxblox: Building 3d signed distance fields for planning,” arXiv, pp. arXiv–1611, 2016.
[6] O. Souissi, R. Benatitallah, D. Duvivier, A. Artiba, N. Belanger, and P. Feyzeau, “Path planning: A 2013 survey,” in Proceedings of 2013 International Conference on Industrial Engineering and Systems Management (IESM). IEEE, 2013, pp. 1–8.
[7] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” IEEE transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996.
[8] S. M. LaValle, “Rapidly-exploring random trees: A new tool for path planning,” 1998.
[9] B. Donald, P. Xavier, J. Canny, and J. Reif, “Kinodynamic motion planning,” Journal of the ACM (JACM), vol. 40, no. 5, pp. 1048–1066, 1993.
[10] M. Lan, S. Lai, Y. Bi, H. Qin, J. Li, F. Lin, and B. M. Chen, “Bit*-based path planning for micro aerial vehicles,” in IECON 2016-42nd Annual Conference of the IEEE Industrial Electronics Society. IEEE, 2016, pp. 6079–6084.
[11] J. A. Starek, J. V. Gomez, E. Schmerling, L. Janson, L. Moreno, and M. Pavone, “An asymptotically-optimal sampling-based algorithm for bi-directional motion planning,” in 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2015, pp. 2072–2078.
[12] E. Glassman and R. Tedrake, “A quadratic regulator-based heuristic for rapidly exploring state space,” in 2010 IEEE International Conference on Robotics and Automation. IEEE, 2010, pp. 5021–5028.
[13] A. Perez, R. Platt, G. Konidaris, L. Kaelbling, and T. Lozano-Perez, “Lqr-rrt*: Optimal sampling-based motion planning with automatically derived extension heuristics,” in 2012 IEEE International Conference on Robotics and Automation. IEEE, 2012, pp. 2537–2542.
[14] J. van den Berg, “Extended lqr: Locally-optimal feedback control for systems with non-linear dynamics and non-quadratic cost,” in Robotics Research. Springer, 2016, pp. 39–56.
[15] Z. Zhu, E. Schmerling, and M. Pavone, “A convex optimization approach to smooth trajectories for motion planning with car-like robots,” in 2015 54th IEEE Conference on Decision and Control (CDC). IEEE, 2015, pp. 835–842.
[16] C. Rösmann, F. Hoffmann, and T. Bertram, “Kinodynamic trajectory optimization and control for car-like robots,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 5681–5686.
[17] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and efficient quadrotor trajectory generation for fast autonomous flight,” IEEE Robotics and Automation Letters, vol. 4, no. 4, pp. 3529–3536, 2019.
[18] V. Usenko, L. von Stumberg, A. Pangercic, and D. Cremers, “Real-time trajectory replanning for mavs using uniform b-splines and a 3d circular buffer,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 215–222.
[19] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in 2011 IEEE International Conference on Robotics and Automation. IEEE, 2011, pp. 2520–2525.
[20] A. Babaei and A. Karimi, “Optimal trajectory-planning of uavs via b-splines and disjunctive programming,” arXiv preprint arXiv:1807.02931, 2018.
[21] L. Biagiotti and C. Melchiorri, “B-spline based filters for multi-point trajectories planning,” in 2010 IEEE International Conference on Robotics and Automation. IEEE, 2010, pp. 3065–3070.
[22] J. E. Bobrow, “Optimal robot plant planning using the minimum-time criterion,” IEEE Journal on Robotics and Automation, vol. 4, no. 4, pp. 443–450, 1988.
[23] A. Guttman, “R-trees: A dynamic index structure for spatial searching,” in Proceedings of the 1984 ACM SIGMOD international conference on Management of data, 1984, pp. 47–57.
[24] ellipsoid. [Online]. Available: https://people.sc.fsu.edu
[25] G. Kulathunga, R. Fedorenko, S. Kopylov, and A. Klimchik, “Real-time long range trajectory replanning for mavs in the presence of dynamic obstacles,” 2020.
[26] J. van den Berg, “Iterated lqr smoothing for locally-optimal feedback control of systems with non-linear dynamics and non-quadratic cost,” in 2014 American Control Conference. IEEE, 2014, pp. 1912–1918.
[27] G. Marro, D. Prattichizzo, and E. Zattoni, “Geometric insight into discrete-time cheap and singular linear quadratic riccati (lqr) problems,” IEEE Transactions on Automatic Control, vol. 47, no. 1, pp. 102–107, 2002.