CC BY
26Russian Journal of Nonlinear Dynamics, 2021, vol. 17, no. 4, pp. 491-505. Full-texts are available at http://nd.ics.org.ru DOI: 10.20537/nd210410
NONLINEAR ENGINEERING AND ROBOTICS
MSC 2010: 12Y05, 49Kxx, 68W27
Path Planning Followed by Kinodynamic Smoothing for Multirotor Aerial Vehicles (MAVs)
Any obstacle-free path planning algorithm, in general, gives a sequence of waypoints that connect start and goal positions by a sequence of straight lines, which does not ensure the smoothness and the dynamic feasibility to maneuver the MAV. Kinodynamic-based motion planning is one of the ways to impose dynamic feasibility in planning. However, kinodynamic motion planning is not an optimal solution due to high computational demands for real-time applications. Thus, we explore path planning followed by kinodynamic smoothing while ensuring the dynamic feasibility of MAV. The main difference in the proposed technique is not to use kinodynamic planning when finding a feasible path, but rather to apply kinodynamic smoothing along the obtained feasible path. We have chosen a geometric-based path planning algorithm "RRT*" as the path finding algorithm. In the proposed technique, we modified the original RRT* introducing an adaptive search space and a steering function that helps to increase the consistency of the planner. Moreover, we propose a multiple RRT* that generates a set of desired paths. The optimal path from the generated paths is selected based on a cost function. Afterwards, we apply kinodynamic smoothing that will result in a dynamically feasible as well as obstacle-free path. Thereafter, a b-spline-based trajectory is generated to maneuver the vehicle autonomously in unknown environments. Finally, we have tested the proposed technique in various simulated environments. According to the experiment results, we were able to speed up the path planning task by 1.3 times when using the proposed multiple RRT* over the original RRT*.
Keywords: RRT*, iLQR, B-spline, OctoMap, ellipsoidal search space
Received March 26, 2021 Accepted October 27, 2021
The work presented in this paper has been supported by Innopolis University, Ministry of Education and National Technological Initiative, within the framework of creation of the Center for Technologies in Robotics and Mechatronics Components.
Geesara Kulathunga [email protected] Dmitry Devitt [email protected] Roman Fedorenko [email protected] Alexandr Klimchik [email protected]
Center for Technologies in Robotics and Mechatronics Components, Innopolis University ul. Universitetskaya 1, Innopolis, 420500 Russia
G. Kulathunga, D. Devitt, R. Fedorenko, A. Klimchik
1. Introduction
With the recent research advances in microcontroller technology and sensors' capabilities, a new era has begun for MAVs. In recent years, such devices have been applied in many fields including delivery, farming and cinematography. Since most of the listed scenarios have to perform real-time operations, optimal motion planning receives a lot of attention and plays a key role. Optimal motion planning consists of two parts: planning an obstacle-free path and further optimizing such a path, ensuring the provided constraints to obtain an optimal control policy to navigate MAV appropriately. 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 fulfill all the provided constraints. When considering real-time motion planning, geometric-based path planning followed by path parameterization is a well-adapted technique. However, in such an approach, path parameterization may fail despite selecting a geometrically shortest path.
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 path planning algorithm, namely, RRT* [1], which comes under geometric-based path planning that guarantees asymptotic optimality. On the other hand, RRT* may experience unpredictable performance issues due to its randomized behavior. Thus, there is no guarantee of the computational cost. In the proposed solution, we have a modified original RRT* [1] introducing an adaptive search space and a steering function that helps to increase the consistency of the planner. In addition, we propose a multiple RRT* that generates a set of desired paths where the optimal path is chosen based on a cost function. Finally, we apply kinodynamic smoothing, which will result in a dynamically feasible as well as obstacle-free path. Thereafter, a b-spline based trajectory is generated to maneuver the MAV autonomously.
Our Contributions:
1. Modifying the original RRT* algorithm introducing an adaptive search space and a steering function that helps to increase the consistency of the planner.
2. Proposing the planning of a horizon-based multiple RRT*. In each of the RRT* instances, it finds a feasible intermediate goal location and terminates at a newly generated goal position if the path length exceeds the planning horizon.
3. Applying iLQR (Iterative Linear Quadratic Regulator) smoothing to obtain the optimal obstacle-free path while ensuring the dynamic feasibility of MAV.
2. Related Work
2.1. Environment representation
Understanding the environment in real-time is important for collision-free planning. Hence, keeping track of the environment (map) close to the current position of MAV with a predefined perimeter is necessary provided that the map is updated in an incremental fashion. 3D voxel grid is one of the ways to represent the environment in a compact fashion. Each voxel grid stores information about its occupancy complemented with constant access time for retrieving any element. In spite of preceding advantages, it requires quite high memory-footprint. Hornung. et al. [2, 3] proposed Octomap, which uses probabilistic occupancy estimation to construct the map, namely, octree for storing the information. In the process of map building, random measurements, e.g., reflections, dynamics obstacles should be considered. 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.
2.2. Path planning
For path planning, a better understanding of the environment, as well as how environment is constructed and updated over time, is equally important. Initially, the path planing algorithm assumes an inaccessible area as a traversable area. Once MAV starts moving towards the desired direction, MAV starts exploring the unseen area and a feasible trajectory must be reconstructed, followed by replanning. 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 the fact that the path satisfying real-time constraints is not planned effectively and that the complexity increases 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 that can be categorized under sampling-based techniques.
2.3. Kinodynamic path planning
Most of the trajectory planning algorithms perform path planning followed by feasible trajectory generations as a two-step pipeline. Such a procedure may be problematic when it comes to replanning in which the path planner is 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 by adding kinody-namic capabilities. However, real-time kinodynamic motion planning is seen as an open problem due to high computational cost. Moreover, a path generated by the path planner should be smooth. Frequently, spline-based techniques are utilized for smoothing without considering vehicle dynamics and obstacle constraints. Such trajectories are often infeasible trajectories. Such an infeasible trajectory generation can be avoided by applying kinodynamic path planning algorithms. One of such algorithms is the kinodynamic RRT, which was proposed by LaValle. et al. [12] and is applied for a hovercraft in a simulated environment. The most challenging task of the kinodynamic RRT* is to design an appropriate steering function [1, 13]. It is responsible for taking the control inputs and the current state and the preceding state (parent state), and connecting to the next state while ensuring dynamic feasibility.
To generate optimal control policy for a given MAV, which is utilized in kinodynamic planning, various techniques have been proposed. Linear Quadratic Regulator (LQR) was suggested by Glassman and Tedrake [14] for the kinodynamic RRT planner to generate an optimal control policy. Alejandro Perez, Robert Platt Jr [13] extended the preceding idea for RRT* while linearizing the nonlinear system dynamics at each new sample point. Moreover, they used an infinite horizon LQR policy to obtain optimal control inputs. Instead of using an infinite horizon, Jur and Berg [15] proposed finite horizon iterative LQR and extended LQR smoothing techniques considering nonlinear dynamics and nonquadratic cost for optimal kinodynamic motion planning.
2.4. Path smoothing
Path planning yields a sequence of waypoints which are connected by a set of straight lines and sharp turns. Hence, a path may not be desirable for navigation due to three constraints: geometric continuity, safety and dynamic feasibility. To fulfill these constraints, the path is
smoothed by ensuring those constraints. In general, there are three types of smoothing techniques: interpolate based, special curve based and optimization based. Polynomial interpolation, cubic splines and b-spline are good examples of interpolate based techniques. Dubin's curve and clothoids are some of the special curves which can be used for smoothing. However, none of those two technique takes into account dynamic feasibility and safety. Optimization-based approaches can satisfy all three preceding constraints. The authors of [16] proposed a convex elastic smoothing (CES) algorithm for trajectory smoothing as a convex optimization problem. Timed Elastic Band (TEB Planner) [17] is a kinodynamic planner which locally optimizes the trajectory while considering provided constraints. Zhou et al. [18] propose a kinodynamic local planner based on A*, which works quite aggressively.
2.5. Trajectory generation
There are several approaches for trajectory generation, including path planning followed by smoothing [19], motion primitive [20, 21] based, optimization-based [22, 23], etc. Path planning followed by smoothing is one of the widely adapted techniques among the preceding techniques. As explained in the previous section, the output of the path planner will be a sequence of waypoints in SO(3) that connects the start and goal positions coarsely. Such waypoints constitute the input to the trajectory generator. The trajectory generator approximates the desired path with time-based waypoints, which consists of position, velocity, acceleration, yaw, pitch and roll angles in SO(3) for a given time index. The newly generated trajectory must be feasible. If the generated trajectory satisfies all the limitations and constraints that are imposed by the dynamics of the vehicle and constraints on the environment, such a trajectory can be regarded as a feasible trajectory.
B-spline [24], minimum-snap [25] and its variants have widely been used for trajectory generation in the recent past. The initial stage of trajectory generation related work linearizes the model around the hover conditions which cannot be stabilized for reasonably high roll and pitch angles [26]. Minimum-snap trajectory generator is one of the first introduced techniques for trajectory generation in the tightly constrained setting. Minimum-snap can work with nonlinear controllers while satisfying provided constraints (e. g., velocities, acceleration, input, etc) and ensuring the safe trajectory control sequences with respect to time. Besides, Minimum-snap is suitable for real-time based applications. On the other hand, spline functions, i. e., B-spline [27, 28], are extensively used for trajectory generation for several reasons. Clear geometric representation is very important for trajectory generation in 3D space. B-splines are much simpler from the computational point of view because local changes in the trajectory can be made quickly and easily without recomputing the entire trajectory [29]. Furthermore, if it is required to get derivatives higher than the second order (e. g., jerk, snap), this can be done by changing the order of the B-splines [30].
3. Methodology
The proposed framework is depicted in Fig. 1, whereas the workflow is shown in Fig. 2. The following sections explain in detail how each of the components contributes to acceptable system functioning.
3.1. Environment Representation
Since we use a depth camera for understanding the environment, it is necessary to build an incremental map because the camera has a limited field of view (FoV), i.e., (85.2' x 58' x 94'), which is not enough for planning. Thus, we feed the camera depth map into the Octomap server. The Octomap server constructs the map of the environment incrementally. Thereafter, the point
not success
Fig. 1. Initially, the vehicle stays in the wait state until the goal position is provided. If the goal position is within the map and there are no obstacles at the goal position, its state will change into Gen, which will find an optimal trajectory between the current position and the goal position. Afterwards, the state will change into the Exec state until it reaches the goal position. If there are obstacles close to the vehicle, it retries to generate a new trajectory. This process continues until MAV navigates to the goal position. Besides, Exec state can be changed into Wait state if there are some sensor malfunctions that caused the sensor streams interruption. The PD (P-proportional, D-derivative) regulator takes actual and desired odometries and calculates the estimated velocity and yaw angle for controlling the vehicle
Depth map
OctoMap Builder
LQR
; Smoothing
Odometry
Control Command
PD Regulator
Fig. 2. The sequence of steps of the proposed framework. The depth camera works at 6 Hz, whereas the tracking camera works at 20 Hz. The depth map and odometry constitute an input for the Octomap server, which builds the map incrementally. Based on the output of the Octomap server, the instance map is constructed, which is utilized by Multiple RRT* for generating an optimal obstacle-free path. Afterwards, iLQR smoothing followed by trajectory generation is applied to the optimal path. Finally, the PD regulator is employed to synchronize the generated and actual trajectories
cloud around the current position of the MAV is extracted. RTree [31] is constructed from an extracted point cloud which represents the instance map of the environment.
3.2. Adaptive search space
RRT* is a well-known technique for path planning in high-dimensional spaces, i.e., 3D, in which the search space defines the whole map of the environment in the default setting. We
Algorithm 1: Generating points over the interior of the search space
1 input data: n points to be generated, r principal semiaxes, R rotation matrix, c center position
2 cols = 0, np = 0, rmin = min(r), M = zero(3, n)
3 if ' min == rcc then
4 | h = 2 •
5 end
6 if »min = =
7 | h = 2 •
8 end
9 else
2n+i' ni = n> nj = t: ■ n>~ =
k r
ry then
ry rx rz
n I -, ■ Ua = n, U; = — ■ n, nh = -*- ■ n
2n+1 J ' i ry ' k rx
h = 2 • 2^+T> nk = n> ni =
z = cz + k • h
for j = 0, ..., n.j do
y = cy+j ■ h
for I (I. ..., rr, do
x = cx + i ■ h M(0) = (x, y, z) for / = 0, 1, 2, ..., 7 do if / == 0 || / == 1 || / :
np = 0
10
11 end
12 for k = 0.....n,, do
13
14
15
16
17
18
19
20 21 22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37 end
4 then
end
if / < 0 then
| M(cols + l) = (2c.j, - M(np)x, M(np)y, M(np)z) ■ R end
if 1 < / < 2 then
| M(cols + l) = (M(np)x, 2cy - M(np)y, M(np)z) ■ R end
if / ^ 3 then
| M(cols + l) = (M(np)x, M(np)y, 2c. - M(np)z) ■ R end
cols+ = 8
end
end
end
r
r
n
r
r
y
n, n =
n
J r
r
have made several changes to reduce the execution time and increase the consistency. In most situations, RRT* is utilized for global path planning, not for local planning, especially replanning. On the contrary, in this study, we use RRT* for local replanning. Thus, ensuring consistency of consecutive paths is required. One way to improve consistency is to generate proper random samples and then to define the optimal local search space which is closer to the current position of the vehicle. Considering the preceding facts, we proposed a deterministic way of generating search space, which eventually helps to improve the consistency of the planner significantly. In spite of having the capability of generating an asymptotically optimal path, it might not be consistent with the preceding path.
Search space is constructed as an ellipsoidal search space (spherical or oblate spheroid or prolate spheroid) whose principal axes are given by the start and goal positions of the trajectory. In general, random samples can be generated within the constructed ellipsoid, but we propose a deterministic way of generating sample points (Algorithm 1) similar to1 (see Fig. 3) while considering constraints of the traversable space. Such a deterministic representation helps to reduce the execution time. Since search space is changed according to the traversable space around the current position of MAV, we call the search space an adaptive search space. The RRT* planner picks points randomly from the adaptive search space.
The adaptive search space is defined by
(X - cj2 (y - Cy)2 {z - cz)2 (r,)2 + (r,)2 + (r,)2
+ , y2 + , 2 =1> (3-1)
where c is the middle position between the start position xstart and the goal position xgoal of the path and r = (rx, ry, rz) is determined by
r = xgoal — xstart, (3-2)
where rx = max(4.0, rx), ry = max(4.0, ry) and rz = max(4.0, rz); the values of rx, ry, and rz have been set to 4.0 m at max, assuming that the maximum area which covers the search space is sufficient to have 4.0 m at max in each direction. Once r is obtained, the search space orientation is rotated around r. For the procedure of obtaining such a rotation matrix R, we have used a method that is proposed in [32].
3.3. Multiple RRT*
We have used multiple RRT* instances (see Fig. 3) using a thread pool that utilizes multi-cores/multiprocessors, resulting in N number of paths being generated, to further improve the consistency of path planning. N is a configurable parameter. It is better to select N as the number of cores in the embedded computer because the thread pool is created in the phase of algorithm initialization with the number of threads that is equal to the number of system cores, which helps to improve performance.
Initially, select a path that has the lowest cost as the optimal path based on the following
cost:
M -l
Cost = || (xm — xgoal) | + ^ || (xm — xm+l) || (3.3)
m=1
where xm depicts the mth waypoint of the selected path in which it consists of M number of waypoints. It should be mentioned that cost is not the only factor that should be considered when selecting a path. We have to check the safety of the path. If the chosen path is 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 the MAV starts moving on the trajectory, next time it will start from the lowest cost. If the distance between the start and goal positions is larger than a predefined value (planning horizon), RRT* will return the path up to the planning horizon. This is the procedure for selecting an optimal path from the generated paths.
1 Ellipsoid fitting https://people.sc.fsu.edu
Fig. 3. Multiple RRT* can generate a given number of paths between the start and goal positions (i.e. four paths) in this scenario
3.4. Path smoothing via iLQR
An optimal path that is chosen from the preceding step is smoothed while considering obstacles around the MAV. The optimal path consists of a set of waypoints that connect the start position and the goal position by a set of straight-lines. Afterwards, take each consecutive three waypoints, and get midpoints between the first and second waypoints, and the second and third waypoints and apply iLQR between those two midpoints. iLQR is solved as a finite horizon (N steps) optimization problem. The N number of steps at max is used to smooth the path.
We have used the same MAV model [33]. Let the system state space be x = [pTvTrTwT] e e R"*, where p, v, r and w stand for the position (m), velocity (m/s), the orientation about the r axis by angle |r| (rad) and the angular velocity (rad/s), respectively, in R3. The size of the state space (x) is equal to twelve (nx) and the system has four (nu) control inputs: u = = [ul, u2, u3, u4]. The MAV continuous-time dynamics X = fc(x(t), u(t)) is described by
P = v,
(u1 — u2 + u3 + u4) exp([r]) * e3 — kvv
v = —ge3 +
!r i
r = w + -[rjw +
1 -
';ton i _
m
H^ l..,2„„ (3-4)
¿ftBl'l'»
2L'J~ ' |r|2
W = J -1(p(u2 — u4)e1 + p(u3 — u1 )e2 + km (u1 — u2 + u3 — u4)e3 — [w])Jw,
where e^ i = 1, 2, 3 denotes the basis of the local coordinate frame, g = 9.8 m/s2 stands for gravity, kv = 0.18 and km = 0.020 are constants, m = 0.67 kg, J = 0.056/ (kg m2), and p = = 0.017 m are the mass, moment of inertia, and the distance from the center of the vehicle to the center of the rotor, respectively. [a] depicts the skew-symmetric matrix notation. Afterwards, it is needed to define an optimal control policy that is utilized for generating a path between two given positions while ensuring the MAV dynamics and environmental constraints. Since the
(3.6)
continuous-time system dynamics is known, the discrete-time system dynamics can be formulated
as
x(t + 1) = fd(x(t), u(t)), x(t) G Rn*, u(t) G Rn-, (3.5)
where fd(x(t), u(t)) is determined by the 4th-order Runge-Kutta integrator as follows:
dt
fdMt), u(t)) = x(i) + — • (kl + 2.0 • k'2 + 2.0 • k3 + k4),
6. 0
k1 = fc(x(t), u(t)), k2 = fc(x(t) + 0.5 • dt ■ k1, u(t)), k3 = fc(x(t) + 0.5 ■ dt ■ k2, u(t)), k4 = fc(x(t)+ dt ■ k3, u(t)),
where dt is set to 0.1 s. To obtain an optimal control policy, the following control objective (3.7) is minimized:
N -l
ct(xk(t), uk(t)) + cn(xn(t)), (3.7)
k=0
where ct and cn correspond to the local and final cost, respectively. Then the optimal control policy (nt(x(t)) = [uQ, uQ, ..., uN_ 1]) can be calculated as follows:
N -1
minimize V q(xk(t^ uk(t)) + cn (xN(t)) Uo,..., un-1 k=o
subject to xi(t) = fd(x0(t), u0(t)), x2(t) = fd(xi(t), ui(t)),
xN(t) = fd(xN- 1(t), 1(t)^
(3.8a)
(3.8b) (3.8c) (3.8d) (3.8e)
where x0(t) is the starting position where the nt(x(t)) is calculated. iLQR provides a close-form solution for (3.8a). In iLQR, ct and Cn are determined as follows:
Ct(x(t), u(t)) =
T x(t) u (t)_
4
^t Rt
x(t) u(t)
+
x(t) u(t)
T
qt
(3.9)
M
^ 0 in
where Qt e Mra-xn*, Rt G , Pt g , qt e Rnx and rt G and
which Ql and Rt are positive-definite matrices [15]. Knowing the cost estimation (3.9), discrete-dynamics (3.5) can be rewritten as follows:
x(t + 1) = fd(x(t), u(t)) = Atx(t) + 5tu(t) + Ct(x(t), u(t)) yt,
(3.10)
where At G Rn^xn^, Bt G Rn^xn«, and ct G Rnx. Since the MAV dynamics is nonlinear at each considered discrete time instance t, the quadrotor dynamics fc(x(t), u(t)) is linearized (3.11) and we obtain At and Bt:
At =
dfc(x(t), u(t)) dx(t)
Bt =
9/c(x(t), u(t)) du (t)
(3.11)
1
2
r
t
We can redefine (3.9) in the following way:
ct(x(i), u(t)) = -v{ty Mtv(t) + v(ty mt.
(3.12)
To apply iLQR, it is required to provide MN and mN for the terminal state (the end state of horizon N). Then ct(x(t), u(t)) (Mt £ Rnxxnx > o and mt £ Rnx are computed by using backward recursion with the help of the Riccati equation [34] for N — 1, ..., 0. For example,
Mt = Qt + AT Mt+lAt — (Pt + BT Mt+1At)T (Rt + BT St+lBt)-l(Pt + BT Mt+l At), mt = qt + AT mt+i + AT Mwct — (Pt + BT M+ At )T (Rt + BT M+ Bt)-1 et,
where et = rt + B]Tmt+1 + Bf Mt+1 ct. Thus, the optimal control policy nt(x(t)) has the following linear form:
nt(x(t)) = {—(Rt + BTMt+iBt)-1(Pt + BTMt+1 At)) xt + —(Rt + BTM+B)-1 et. (3.14)
In this way, Mt and mt can be calculated starting from t = N — 1 until t = 0. Besides, obstacles cost £ exp(—di(x(t))) are being added into ct as proposed in [15], where di(x(t)) is the sign
i
distance to the center of each obstacle. di is calculated with the help of RTree by retrieving close obstacles to the projected trajectory at each time step considered. Afterwards, we applied the iLQR smoothing technique between consecutive midpoints of two adjacent waypoints that are generated from RRT*. An example scenario is shown in Fig 4. Finally, the b-spline trajectory is generated using the smoothed path in which the MAV is maneuvered safely.
Fig. 4. The proposed multiple RRT* is capable of selecting an optimal path out of the generated paths. Then the optimal path is smoothed by iLQR smoothing
3.5. B-Spline Trajectory Generation
Multiple RRT* followed by iLQR smoothing gives a sequence of waypoints (ns number of waypoints), which are guaranteed to be in the obstacle-free zone. Let those waypoints be the control points (ci)£ 1 £ R3 for a uniform b-spline curve fb(t), which is a function of time, with
a degree d (in our case, d equals 3). To generate the knot vector (t^n^d, we set the step size (step_size) as 0.1 s, then the knot vector is calculated as follows:
n +d+1 \ (—d + i) ■ step_size, t ^ d, [ ti-1 + step_size, t > d.
Knowing the control points and the knot vector, we can define fb(t) by
n
fb(t) = E ciBi,d(t), (3-15)
i=d+1
where Bi d is given as
BUt) = + -h±m_LBi+ld_l{t) (3.i6)
ti+d ti ti+d+1 ti+1
with
1, ti <t< ti+1, Bi 0(t) = < i i+ (3.17)
I 0, otherwise.
The derivative of B-spline is also a b-spline whose order becomes d-1. Let ci be the fth derivative of control points ci which is defined by
c, = . (3.i8)
ti+d+1 ti+1
In this way, both velocity vt G R3 and acceleration at G R3 components are calculated for a time index t that yields the desired MAV control input. The calculation of the desired yaw angle at is relaxed when generating a trajectory using b-spline; this is due to MAV yaw angle changes when avoiding obstacles. Thus, the desired rate of yaw angle is estimated as follows.
Algorithm 2: Desired yaw angle estimation
1 input: vt current velocity
2 output: at desired yaw rate
vt
3 V = T-tt
llvtll
{atau2(vy, vx), vy > 0,
atan2(vy, vx) + 2n, otherwise
3.6. PD regulator
The PD regulator is employed for synchronizing the desired position from the planner and the actual MAV position. Since we used a px4 controller2 on MAV, one way to control the quadrotor is by sending the desired velocity vt and the yaw rate at. Hence, the expected velocity and the yaw rate are estimated as given in Algorithm 3, where kp = 0.8, kd = 0.27, and ka = 2.5 are PD regulation parameters and x*, v*; ¡i = x, y, z; * = a, d gives both the current position and the velocity along each axis to the actual and the desired trajectories, respectively. When the expected velocity exceeds its upper (vmax) or lower (vmin) boundaries, the expected value is clipped to the respective boundary value.
2px4 controller https://dev.px4.io/v1.9.0
Algorithm 3: PD regulator
1 input: x" g R3 actual position, xd g R3 desired position, a G R desired yaw rate, aa G R actual
yaw rate, v g R3 actual velocity
2 output: v g R3 estimated velocity, a g R estimated yaw rate
3 Sx = x" — xd
4 Vx = clip{ÔXx ■ kp - vx ■ kd, Vmin, vmax)
5 Vy = clip{ÔTiy ■ kp - Vy • kd, Vmin, Vmax)
6 \z = clip{ôxz ■ kp - vz ■ kd, vmin, vmax)
7 â = o.t — o"'
f — = 27T, a > 7T,
8 a = <
[ + = 27T, a < —it
9 â = ka ■ â,
20 40 60 80 Map updating time [ms] per point cloud scan
(a) 3D Ring Buffer [24]
20 40 60 80 100 Map updating time [ms] per point cloud scan
(b) Euclidean Signed Distance Field [18]
).0 0.5 1.0 1.5 2.0 2.5 Map updating time [ms] per point cloud scan
(c) Proposed instance map building approach
Fig. 5. Comparison of the map updating time (ms) per point cloud scan. The range of the map set for 4 m around the quadcopter current position. Subplots (a), (b), (c) show the updating time as histogram
4. Experimental Results
The sparse matrix library, namely, Eigen, was used to implement the proposed planner using C++11 while enabling the GNU C++ compiler optimization level to -O2. All the simulated experiments were conducted on a computer with (Intel(R) Core(TM) i7-7500U CPU @ 2.70 GHz CPU and 8 GB RAM). In this section, we present a qualitative and quantitative comparison of the proposed local replanner. Firstly, we compare our proposed instance map building with two other techniques, i.e., 3D Ring Buffer (Fig. 5a), ESDF (Fig. 5b). Secondly, the performance of the improved RRT* is compared with A* and the original RRT*. Thirdly, the complete system is evaluated in various simulated environments.
The proposed instance map, which is using the RTree [31] method, is constructed when the depth map is available at 6 Hz. We have adopted this approach to reduce the execution time; the approach is used for map building on the MAV. In the proposed approach, it is necessary to keep two instance maps (current and previous); first, the previous map is utilized by the planner, and once the current map is built, the previous map is replaced with the current map. The main advantage of the proposed map building in comparison with the above-mentioned methods [18, 24] is that, despite building an incremental map, the proposed approach will save computational costs and execution time while maintaining an accuracy similar to the above-mentioned approaches.
We have compared the mean computation time of the improved RRT* with the original RRT* and A*; further we calculated the mean computation time of the main five operations that are involved in the proposed planner as percentage values, as shown in Fig. 6. All three
Fig. 6. Mean computation times of the main four operations involved in the proposed planner as percentages
algorithms utilize 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 the mean computation time (Table 1). We were able to reduce the computation time considerably. Besides, the improved RRT* has a constrained search space, it helps to improve the consistency of the path finding as we detailed in the Methodology section.
Table 1. Mean computation time for path finding under which same search space utilized for 100 different trials
A* Original RRT* Improved RRT*
Mean computation time (s) 1.9612 1.2512 0.9621
Finally, we have conducted a few experiments in different simulated environments. Figure 7a shows one of the selected simulated environments for testing the behavior of the planner. When the distance between the goal and start positions is less than the predefined horizon (Fig. 7b), it generates a complete path, otherwise it generates a path up to the horizon as shown in Fig. 7c. Hence, the proposed planner works well under different environment conditions.
Fig. 7. An improved RRT* generates an N number (e. g., 4) of paths and selects the optimal path. Afterwards, iLQR smoothing is applied on the selected path that is shown in orange color. The other paths are shown in blue, dark blue and green, respectively, whereas the small red-colored sphere is the goal position. (a) A sample environment that is used for testing the proposed replanner. (b) When the Euclidean distance between the start and goal positions is less than the prediction horizon of the RRT*, it will generate a full path between the start and goal positions (c) When the Euclidean distance between the start and goal positions is higher than the horizon of the RRT*, it will generate a path between the start and the horizon as shown here. The nominal path is reprojected whenever close obstacles are detected
5. Conclusion
We have presented a framework for path planning which is followed by kinodynamic smoothing while ensuring the vehicle dynamics feasibility for MAVs. We have chosen "RRT*" for path planning in which several improvements have been done to reduce the planning time, which helps to employ RRT* as a local path finding algorithm. Kinodynamic smoothing ensures the dynamic feasibility and an obstacle-free path. Finally, we have validated the proposed framework in various simulated environments to check the behavior of the planner. Our validation result shows the performance of the planner. In our future work, we are going to improve the efficiency of the path planner.
References
[1] Karaman, S. and Frazzoli, E., Sampling-Based Algorithms for Optimal Motion Planning, Int. J. Robot. Res., 2011, vol. 30, no. 7, pp. 846-894.
[2] Hornung, A., Wurm, K. M., Bennewitz, M., Stachniss, C., and Burgard, W., OctoMap: An Efficient Probabilistic 3D Mapping Framework Based on Octrees, Auton. Robot., 2013, vol. 34, no. 3, pp. 189206.
[3] Steinbrucker, F., Sturm, J., and Cremers, D., Volumetric 3D Mapping in Real-Time on a CPU, in IEEE Internat. Conf. on Robotics and Automation (ICRA'2014), pp. 2021-2028.
[4] Nietëner, M., Zollhofer, M., Shahram, I., and Stamminger, M., Real-Time 3D Reconstruction at Scale Using Voxel Hashing, ACM Trans. Graph., 2013, vol. 32, no. 6, Art. 169, 11 p.
[5] Oleynikova, H., Taylor, Z., Fehr, M., Nieto, J., and Siegwart, R., Voxblox: Building 3D Signed Distance Fields for Planning, arXiv:1611.03631 (2016).
[6] Souissi, O., Benatitallah, R., Duvivier, D., Artiba, A., Belanger, N., and Feyzeau, P., Path Planning: A 2013 Survey, in Proc. of the 2013 Internat. Conf. on Industrial Engineering and Systems Management (IESM), 8 p.
[7] Kavraki, L.E., Svestka, P., Latombe, J.-C., and Overmars, M.H., Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces, IEEE Trans. Rob. Autom., 1996, vol. 12, no. 4, pp. 566-580.
[8] LaValle, S.M., Rapidly-Exploring Random Trees: A New Tool for Path Planning, Iowa State Univ. Technical Report 11, 1998.
[9] Donald, B., Xavier, P., Canny, J., and Reif, J., Kinodynamic Motion Planning, J. ACM, 1993, vol. 40, no. 5, pp. 1048-1066.
[10] Lan, M., Lai, Sh., Bi, Y., Qin, H., Li, J., Lin, F., and Chen, B.M., BIT*-Based Path Planning for Micro Aerial Vehicles, in IECON 2016: Proc. of the 42nd Annual Conf. of the IEEE Industrial Electronics Society, pp. 6079-6084.
[11] Starek, J. A., Gomez, J.V., Schmerling, E., Janson, L., Moreno, L., and Pavone, M., An Asymptotically-Optimal Sampling-Based Algorithm for Bi-Directional Motion Planning, in IEEE/RSJ Internat. Conf. on Intelligent Robots and Systems (IROS'2015), pp. 2072-2078.
[12] LaValle, S.M. and Kuffner, J. J., Jr., Randomized Kinodynamic Planning, Int. J. Robot. Res., 2001, vol. 20, no. 5, pp. 378-400.
[13] Perez, A., Platt, R., Konidaris, G., Kaelbling, L., and Lozano-Perez, T., LQR-RRT*: Optimal Sampling-Based Motion Planning with Automatically Derived Extension Heuristics, in 2012 IEEE Internat. Conf. on Robotics and Automation, pp. 2537-2542.
[14] Glassman, E. and Tedrake, R., A Quadratic Regulator-Based Heuristic for Rapidly Exploring State Space, in 2010 IEEE Internat. Conf. on Robotics and Automation, pp. 5021-5028.
[15] van den Berg, J., Extended LQR: Locally-Optimal Feedback Control for Systems with Non-Linear Dynamics and Non-Quadratic Cost in Robotics Research, M.Inaba, P. Corke (Eds.), Springer Tracts Adv. Robot., vol. 114, Cham: Springer, 2016, pp. 39-56.
[16] Zhu, Zh., Schmerling, E., and Pavone, M., A Convex Optimization Approach to Smooth Trajectories for Motion Planning with Car-Like Robots, in Proc. of the 54th IEEE Conf. on Decision and Control (CDC'2015), pp. 835-842.
[17] Rosmann, Ch., Hoffmann, F., and Bertram, T., Kinodynamic Trajectory Optimization and Control for Car-Like Robots, in IEEE/RSJ Internat. Conf. on Intelligent Robots and Systems (IROS'2017), pp. 5681-5686.
[18] Zhou, B., Gao, F., Wang, L., Liu, Ch., and Shen, Sh., Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight, IEEE Robot. Autom. Lett., 2019, vol. 4, no. 4, pp. 35293536.
[19] Biagiotti, L. and Melchiorri, C., Trajectory Planning for Automatic Machines and Robots, Berlin: Springer, 2008.
[20] Mueller, M. W., Hehn, M., and D'Andrea, R., A Computationally Efficient Algorithm for State-to-State Quadrocopter Trajectory Generation and Feasibility Verification, in 2013 IEEE/RSJ Internat. Conf. on Intelligent Robots and Systems, pp. 3480-3486.
[21] Paranjape, A. A., Meier, K. C., Shi, X., Chung, S.-J., and Hutchinson, S., Motion Primitives and 3D Path Planning for Fast Flight through a Forest, Int. J. Robot. Res., 2015, vol. 34, no. 3, pp. 357-377.
[22] Zucker, M., Ratliff, N., Dragan, A.D., Pivtoraiko, M., Klingensmith, M., Dellin, Ch. M., Bag-nell, J. A., and Srinivasa, S. S., Chomp: Covariant Hamiltonian Optimization for Motion Planning, Int. J. Robot. Res., 2013, vol. 32, nos. 9-10, pp. 1164-1193.
[23] Oleynikova, H., Burri, M., Taylor, Z., Nieto, J., Siegwart, R., and Galceran, E., Continuous-Time Trajectory Optimization for Online UAV Replanning, in IEEE/RSJ Internat. Conf. on Intelligent Robots and Systems (IROS'2016), pp. 5332-5339.
[24] Usenko, V., von Stumberg, L., Pangercic, A., and Cremers, D., Real-Time Trajectory Replanning for MAVs Using Uniform B-Splines and a 3D Circular Buffer, in IEEE/RSJ Internat. Conf. on Intelligent Robots and Systems (IROS'2017), pp. 215-222.
[25] Mellinger, D. and Kumar, V., Minimum Snap Trajectory Generation and Control for Quadrotors, in 2011 IEEE Internat. Conf. on Robotics and Automation, pp. 2520-2525.
[26] Hoffmann, G., Waslander, S., and Tomlin, C., Quadrotor Helicopter Trajectory Tracking Control, in AIAA Guidance, Navigation and Control Conference and Exhibit (Honolulu, Hawaii, 2008), AIAA 2008-7410, 14 p.
[27] Babaei, A. and Karimi, A., Optimal Trajectory-Planning of UAVs via B-Splines and Disjunctive Programming, arXiv:1807.02931 [math.OC] (2018).
[28] Biagiotti, L. and Melchiorri, C., B-Spline Based Filters for Multi-Point Trajectories Planning, in 2010 IEEE Internat. Conf. on Robotics and Automation, pp. 3065-3070.
[29] Bobrow, J. E., Optimal Robot Plant Planning Using the Minimum-Time Criterion, IEEE J. Robot. Autom., 1988, vol. 4, no. 4, pp. 443-450.
[30] Wang, C.-H. and Horng, J.-G., Constrained Minimum-Time Path Planning for Robot Manipulators via Virtual Knots of the Cubic B-Spline Functions, IEEE Trans. Automat. Control, 1990, vol. 35, no. 5, pp. 573-577.
[31] Guttman, A., R-Trees: A Dynamic Index Structure for Spatial Searching, in Proc. of the 1984 ACM SIGMOD Internat. Conf. on Management of Data, pp. 47-57.
[32] Kulathunga, G, Fedorenko, R., Kopylov, S., and Klimchik, A., Real-Time Long Range Trajectory Replanning for MAVs in the Presence of Dynamic Obstacles, arXiv:2001.03605 (2020).
[33] van den Berg, J., Iterated LQR Smoothing for Locally-Optimal Feedback Control of Systems with Non-Linear Dynamics and Non-Quadratic Cost, in American Control Conference (Portland, OR, USA, June 2014), pp. 1912-1918.
[34] The Riccati Equation, S.Bittanti, A. J.Laub, J.C.Willems (Eds.), Berlin: Springer, 2012.
