Kinodynamic planning for an energy-efficient autonomous ornithopter

Kinodynamic Planning for an Energy-Efficient Autonomous Ornithopter Fabio Rodr´ıguez2Jos´e-Miguel D´ıaz-B´a˜nez2,∗Ernesto Sanchez-Laulhe1 Jes´us Capit´an1An´ıbal Ollero1∗†‡ October 26, 2020 Abstract This paper presents a novel algorithm to plan energy-efficient trajectories for autonomous ornithopters. In general, trajectory optimization is quite a relevant problem for practical applications with Unmanned Aerial Vehicles (UAVs). Even though the problem has been well studied for fixed and rotatory-wing vehicles, there are far fewer works exploring it for flapping-wing UAVs like ornithopters. These are of interest for many applications where long flight endurance, but also hovering capabilities are required. We propose an efficient approach to plan ornithopter trajectories that minimize energy consumption by combining gliding and flapping maneuvers. Our algorithm builds a tree of dynamically feasible trajectories and applies heuristic search for efficient online planning, using reference curves to guide the search and prune states. We present computational experiments to analyze and tune key parameters, as well as a comparison against a recent alternative probabilistic planning, showing best performance. Finally, we demonstrate how our algorithm can be used for planning perching maneuvers online. Keywords: Trajectory optimization, ornithopter, heuristics, nonlinear dynamics. 1 Introduction Unmanned Aerial Vehicles (UAVs) are spreading quite fast for many applications due to their versatility and autonomy. However, they present two main barriers to reach a wider range of applications: (i) flight endurance; and (ii) safety during interactions with people and objects in the environment. For instance, flight endurance is essential in applications like long-range inspection of infrastructures (e.g., power lines). Conventional multi-rotor UAVs do not achieve competitive flight times for those scenarios; and the use of fixed-wing UAVs does not solve the problem completely either, as capabilities for Vertical Take-Off and Landing (VTOL) and hovering in-place are required for accurate inspections. Additionally, most of the aforementioned platforms are not safe enough to interact with people, due to their powerful rotor systems, their blades and the hard materials of their airframes. In order to cope with both issues, endurance and safety, bio-inspired UAVs like flapping-wing vehicles or ornithopters [14, 25, 9, 2] are of interest. These try to imitate birds flying, as birds can travel long distances efficiently. Thus, the main objective of GRIFFIN project 1, which is the one inspiring this work, is the design of bio-inspired flapping-wing UAVs that are able not only to fly but also to perch in order to interact with the environment through manipulation. A key aspect for the development of these ornithopters is to make them able to combine efficiently gliding and flapping phases, as birds do. Gliding allows the UAV to save energy and extend its flight endurance, but flapping is still necessary to increase altitude and to perform perching operations. Therefore, during the trajectory planning process, ornithopters should consider when to transition optimally between flapping and gliding, in order to save as much energy as possible. ∗1GRVC Robotics Lab, University of Seville, Spain. [email protected], [email protected], [email protected] †2Departamento de Matem´atica Aplicada II, University of Seville, Spain. [email protected], [email protected] ‡∗Corresponding author 1GRIFFIN is an Advanced Grant of the European Research Council (https://griffin-erc-advanced-grant.eu). 1 arXiv:2010.12273v1 [cs.RO] 23 Oct 2020 Figure 1: View of our ornithopter prototype. In general, planning optimal trajectories for autonomous ornithopters is a complicated problem. First, these types of vehicles present nonlinear and complex dynamics that need to be taken into account when computing feasible trajectories. Second, the state space should include vehicle’s position, velocities and attitude, which are relevant for gliding and perching operations. Due to these complexities, there is a need for model-based but efficient methods that allow us to compute optimal trajectories with real-time performance. Some works use numerical methods for model-based trajectory planning [30, 16]. For instance, numerical solutions of the Navier-Stokes equations have been used [28], but they are too expensive computationally for real-time implementation. Other approaches use probabilistic motion planners [35, 17] integrating kinodynamic constraints or evolutionary algorithms [23], but again, tractable models are necessary not to exceed computational requirements. Contrary to other related work, in this paper, we propose a novel optimization algorithm for energyefficient trajectory planning with an autonomous ornithopter. We envision the use of flapping-wing UAVs for tasks like surveillance or inspection due to their ability to perform long-endurance flights. Therefore, our main objective is to compute online trajectories that minimize the energy consumption of the ornithopter. Those trajectories have to comply with the ornithopter dynamics, being thus flyable. Then, we assume the existence of lower-level algorithms to control the ornithopter tracking the computed trajectory. For example, control laws for stable longitudinal and lateral flight with actual flapping-wing UAVs in perching operations have been proposed [27]. We compute a tree of dynamically feasible trajectories by using a nonlinear model for the ornithopter motion and applying segmented tail angles and flapping frequencies. The two flight modes of the ornithopter, i.e., flapping and gliding, are modeled with different aerodynamic coefficients, which implies a more complex nonlinear model that depends on the flight mode. Then, we run heuristic tree search pursuing optimal solutions. In order to achieve online planning, computational complexity is alleviated twofold: (i) we propose an ornithopter model with simplified dynamics, to make it computationally tractable; and (ii) some pruning operations to reduce the tree search space and keep it bounded. Even though we use in this paper a 2D model that constraints the ornithopter movement to a longitudinal plane, our algorithm is general and could be applied to 3D trajectory planning given a proper ornithopter model. Moreover, our heuristic solver finds approximate solutions with minimal energy consumption, but we also demonstrate the efficacy of those trajectories regarding final target state achievement. In summary, our main contributions are the following: •We propose a dynamic model for the 2D motion of our prototype ornithopter (see Figure 1). The model is nonlinear and complex, combining the aerodynamic behavior from both gliding and flapping phases. Nonetheless, we show how the model is computationally tractable for online trajectory planning. •We contribute with a new tree-based algorithm for the computation of trajectories that minimize energy consumption and comply with dynamics for the ornithopter. Gliding and flapping operations are integrated for efficient trajectory planning. 2 •Our algorithm can compute online approximated solutions by means of heuristic operations that prune the tree. We design energy-efficient curves for the ornithopter that guide the tree growth and keep the number of tree nodes bounded. •We run extensive computational experiments to analyze our algorithm and tune its key parameters. Moreover, we demonstrate its performance in comparison with another relevant approach from the literature and show a special case study for planning perching maneuvers. The remainder of the paper is organized as follows: Section 2 surveys related work; Section 3 introduces our trajectory planning problem; Section 4 describes the dynamic model for our ornithopter; Section 5 details our algorithm for trajectory planning; Section 6 presents computational experiments to select parameters; Section 7 shows experimental results to better assess the performance of our algorithm; and Section 8 discusses our results and explores future work. 2 Related Work Generally speaking, motion planning for UAVs is a different problem to ours but somehow related. In the literature, the terms motion planning and path planning are usually employed indistinctly to refer to the same problem: given a robot in a workspace with obstacles, find a collision-free path from an initial to a goal configuration. Many methods for motion planning make use of the differential flatness of multicopter systems to generate optimal, continuous-time trajectories represented as polynomials [22, 24, 26]. It can be assured that those trajectories are dynamically feasible given simplified multicopter dynamics. Others [5, 29] use motion primitives to discretize the UAV state space into a connected graph. Then, standard graph search algorithms like A* can be used to find efficient solutions through the graph. However, the common assumptions made by the previous works do not hold for flapping-wing UAVs, where nonlinear, complex dynamics are of utter importance when planning trajectories. Therefore, instead of exploring motion planning approaches more tailored to collision avoidance, we focus on trajectory optimization methods able to take into account nonlinear dynamics in a computationally tractable manner. In general, the trajectory optimization problem for UAVs consists of finding the sequence of control inputs that minimizes a certain cost index, such as energy consumption or flight time; but fulfilling at the same time constraints on the vehicle dynamics. Thus, trajectory planners use the UAV motion equations (typically differential equations) to provide as output time-indexed variables such as positions, velocities and accelerations. A survey on trajectory optimization for UAVs can be found in [7]. A common approach for UAV trajectory planning are numerical methods. In particular, these methods can be classified into direct and indirect methods. Direct methods rely on discretizing an infinite-dimensional optimization problem into a finite-dimensional problem, to apply then nonlinear programming solvers [30]. Indirect methods do the opposite, they first determine optimal control necessary conditions for the problem, and then use a discretization method to solve resulting equations [4]. For instance, [10] propose a discretetime optimization method (with fixed time-step) for ornithopter trajectory optimization, where the objective is minimizing travelled distance. Another discrete numerical framework for solving constrained optimization problems using gradient-based methods is presented in [34]. The technique is applied to energetically optimal flapping using frequency and pitching/heaving trajectories as optimization parameters. A longitudinal model for the dynamics of a bat-like prototype has also been proposed recently [16]. The authors use that model with direct collocation to plan dynamically feasible trajectories in simulation and track them with their actual prototype. The objective is reducing control efforts by minimizing accelerations. The main concern with these numerical methods is that they can face computational and convergence issues for highly nonlinear problems, as the one addressed in this paper. This makes them less suitable for online trajectory planning. An alternative approach to numerical methods are probabilistic planners like Probabilistic Road-Maps (PRM) or Rapidly-exploring Random Trees (RRT). These algorithms are able to tackle high-dimensional planning problems in reasonable computation time by increasingly sampling the state space. Besides, they are probabilistically complete, i.e., they converge to a solution (if it exist) with probability approaching 1. There are also versions like RRT* [18] that achieve optimal solutions in the same asymptotically manner. 3 Algorithms based on RRT* build a tree by connecting the samples from the state space through optimal trajectories (i.e., solving the two-point boundary value problem). However, computing feasible trajectories for kinodynamic systems is an issue. Some works have extended RRT* focusing on simple specific instances of kinodynamic systems [17]. In [35], for example, they propose a kinodynamic RRT* that can cope with any system with controllable linear dynamics. They even apply the algorithm to nonlinear dynamics through linearization, but without convergence guarantees. Other probabilistic methods deal with nonlinear systems more specifically. For instance, connecting tree nodes using trajectories based on splines that are optimized via a nonlinear program solver [31]. Conversely, there exists the alternative of using exclusively control sampling to handle dynamic constraints, rather than resorting to a numerical two-point boundary value problem solver. This approach is followed by the variants Stable-Sparse RRT* [19] and AO-RRT [15]. Convergence is not proven for any of these RRT* modifications. Moreover, the aforementioned probabilistic planners try to be generic solvers. Contrary to that, we propose problem-specific heuristics that allow us to guide more quickly the search of energy-efficient trajectories for an ornithopter. 3 Problem Description In this section, we introduce the optimization problem to solve in order to plan trajectories with an autonomous ornithopter, as well as the main assumptions we made. We assume that we have an autonomous ornithopter with a known model of its dynamics. Then, we are interested in planning optimal trajectories to navigate the ornithopter from an initial to a target state. This means to compute the sequence of control actions that produces a trajectory connecting the two given states that: (i) is dynamically feasible; and (ii) minimizes the total energy consumed by the ornithopter. This can be done by combining flapping and gliding maneuvers. Moreover, we assume that the UAV is flying in an open space and, hence, we do not consider collisions with obstacles. More specifically, let us define the states and control maneuvers for our problem: Definition 3.1. (Flight state) A flight state s= (x, z, u, w, θ, q)describes an ornithopter configuration in a given instant of time, where xand zare the positional values in the plane XZ of the Earth reference frame, uand ware velocity components in the body reference frame, θis the pitch angle and qis the pitch angular velocity. Definition 3.2. (Flight maneuver) A flight maneuver is a control action performed by the ornithopter during its flight at a given flight state. We consider two degrees of freedom to define flight maneuvers: tail deflection, determined by the deflection angle δ(up and down); and wing flapping, determined by the flapping frequency f(including zero value for gliding). According to the previous definitions, a trajectory consists of a sequence of interleaved flight states and flight maneuvers. Our trajectory planning problem is constrained to a 2D movement (XZ plane), as we use a longitudinal motion model for the ornithopter 2. In particular, we define the Earth reference frame as a global frame with the Zaxis (pointing downwards) representing the ornithopter altitude and the Xaxis its longitudinal motion. We also define a body reference frame attached to the ornithopter with Xbpointing forward and Zbdownwards. Both reference frames, together with the state and control variables are depicted in Figure 2. Finally, we made some additional assumptions to simplify the ornithopter dynamics and derive its model: (i) we use the hypothesis of punctual mass; (ii) we assume small flapping amplitudes and thin airfoils to model aerodynamics; and (iii) we consider the aerodynamics centers to be in a fixed position, as movements are of small amplitude. 4 Ornithopter Dynamic Model We describe in this section a motion model based on the one defined in [21], and specifically developed for bio-inspired, flapping-wing UAVs. The model is used to describe longitudinal motion of our ornithopter prototype. 2Note that our trajectory planning method could be applied to 3D as long as there were a complete 3D motion model for the ornithopter. 4 Figure 2: Schematics of the ornithopter with the forces acting on it. Axis XZ represents the Earth frame; axis X0Z0a translation of the Earth frame; and axis XbZbthe body frame. 4.1 Non-dimensional Newton-Euler equations The Newton–Euler equations describe the combined translation and rotational dynamics of a rigid body, considering all existing forces. For a flapping-wing UAV as the one in Figure 2, the equations can be described as follows: 2Mdu dt =U2 b[(CL+ ΛCLt) sin α + (CT−CD−Li −ΛCDt) cos α] −sin θ−2Mqw (1) 2Mdw dt =U2 b[−(CL+ ΛCLt) cos α + (CT−CD−Li −ΛCDt) sin α] + cos θ+ 2Mqu (2) 1 χU2 b dq dt =CLcos(α)−(CT−CD) sin(α) +LΛ[CLt cos(α) + CDt sin(α)] −RHL[CLsin(α)+(CT−CD) cos(α)] (3) dθ dt =q, (4) where αis the angle of attack, defined as α= arctan(w/u), and Ub=√u2+w2the velocity module. M,χ, Λ, Land RHL are characteristic non-dimensional parameters of the UAV. These parameters are obtained by scaling the variables with the characteristic speed, length and time: Uc=r2mg ρS , Lc=c 2, tc=sρSc2 8mg ,(5) where mis the mass of the UAV, ρthe air density, Sthe wing surface, cthe mean aerodynamic chord and gthe gravity acceleration. Figure 2 shows the forces acting on the vehicle, as well as the representative variables and reference frames used. In particular, L,Tand Drepresent the lift, thrust and drag forces due to the wings; Ltand Dtthe lift and drag from the tail; and Dbthe drag due to body friction. All these forces are considered in Equations (1)-(4) by means of the non-dimensional aerodynamic coefficients: CL,CTand CDfor the wing; CLt and CDt for the tail; and Li for the body. Li =Sb SCDb is the Lighthill’s number, with Sbthe body surface and CDb its friction drag coefficient. Section 4.2 explains how to compute the rest of coefficients for the aerodynamic forces of the wing and the tail. 5 4.2 Aerodynamic models First, let us concentrate on the computation of the lift and thrust forces from the wings. Our model considers two modes of flight for the ornithopter: flapping and gliding; and the computation of the aerodynamic coefficients differs from one to another. In both cases, we make approximations assuming very thin airfoils. For gliding, the Prandtl’s lifting line theory, combined with unsteady aerodynamic terms is used. For flapping, the Theodorsen solution [32] defines the lift provided a wing movement with the form h(t) = h0cos (2πft)/2, being hthe vertical position of the reference wing chord during the flapping movement and h0the movement amplitude. In order to model the existing thrust (only existing in flapping mode), we use the Garrick coefficient [13] corrected by [11, 12]. Moreover, we consider finite wing effects by making aspect ratio corrections based on [3], which leads to: CLglide = 2π"α+ 1.5 ˙α−2lw cq Ub!# A A+ 2 (6) CLflap = 2π{(kh0) [G(k) cos 2πft +F(k) sin (2πft)] + α}A A+ 2 +πk2h0cos (2πft)A A+ 1 (7) CTflap = 4 (kh0)2sin (2πft)[F1(k) cos (2πft) −G1(k) sin (2πft)] A A+ 2 −αCLf lap ,(8) where Ais the aspect ratio of the wing, given by A=b2/S, being bthe wingspan and Sthe surface. lwis the distance between the center of gravity and the aerodynamic center of the wing, being positive when the center of gravity is behind the wing. k= 2πf/Ubis the reduced frequency; F(k) and G(k) are the real and imaginary parts of the Theodorsen’s function C(k); and F1(k) and G1(k) are the real and imaginary parts of the function C1(k) defined in [11]. Regarding the lift force generated by the tail, as our bio-inspired design leads to triangular surfaces [33], we use an approximation for delta wings: CLt=πAt 2"(1−εα)α+δ+ 1.5 ˙α−2lt cq Ub!#,(9) where δis the deflection angle of the tail; εαmodels the interference caused by the wing; and Atand ltare, respectively, the aspect ratio of the tail and the distance between the center of gravity and the aerodynamic center of the wing, being defined in the same manner as it was for the wing. In order to consider near stall effects, we saturate all lift coefficients (CLand CLt) for angles greater than 10ofor the wing and 25ofor the tail. Finally, we model the drags from wings and tail, CDand CDt, as the addition of constant friction drags, CD0and CD0t, and induced drags, provided by: CDi=C2 L πA, CDit =C2 Lt πAt .(10) 5 Ornithopter Segmentation-based Planning Approach In this section, we present our method to solve the problem stated in Section 3, i.e., to plan optimal trajectories for an ornithopter that are both dynamically feasible and energy-efficient. As discussed in Section 2, there exist in the literature numerical optimization solvers that can deal with nonlinear systems. They obtain dynamically feasible trajectories by discretizing and integrating the model dynamics, but can suffer from computational complexity and convergence issues for highly nonlinear models. Graph-based approaches are an alternative, where a graph with discrete, connected states is built in order to search for 6 0 50 100 150 200 250 100 80 60 40 20 0 Tree edges Actual trajectory Flight states x(m) z(m) Figure 3: Segmentation of the ornithopter motion for a landing trajectory. Top view, sequence of flight states followed by a real bird. Bottom view, trajectory computed by our method, with three consecutive maneuvers for 250 meters before landing. The trajectory connects the flight states by integrating the dynamic model. optimal trajectories. In particular, probabilistic planners [18] sample the state space increasingly to build these graphs. If the objective is to generate trajectories that are dynamically feasible, the method must ensure that the sampled states are reachable, which can be complex for nonlinear systems. In this paper, we propose a novel graph-based approach that builds a tree to search for energy-efficient trajectories. Instead of taking random samples from the state space, we segment the ornithopter’s actions and integrate its dynamics to generate and connect feasible states, which can later become nodes of the tree. We named our algorithm OSPA, which stands for Ornithopter Segmentation-based Planning Approach. The general idea is as follows. In our approach, we consider a discrete set Mwith the possible flight maneuvers for the ornithopter. Then, given a fixed time step tsand an initial state, we generate a discrete set of reachable flight states by integrating the ornithopter dynamics for time tsand for each maneuver in M. Doing that iteratively, we build a tree Twhose vertices or nodes are flight states and each edge has associated the corresponding maneuver to navigate from one state (node) to the next one. Moreover, each edge has also associated the energy consumed by the corresponding maneuver to transition between its vertices. We store at each vertex state the energy consumption to reach it from the initial state. The final goal is to find a path τthrough the tree Tthat connects the initial and target states and minimizes the total energy required by the ornithopter. Figure 3 illustrates an example on how to segment the trajectory of an ornithopter and the resulting flight states. A landing operation is achieved by a maneuver with flapping involved, followed by two different maneuvers where the ornithopter is only gliding. Algorithm 1 provides an overview of the procedure followed by OSPA. The algorithm receives as input: •the initial and final states s0and sf, respectively; •the discrete set of maneuvers M; •the time step ts; •pruning parameters kdand kw. The set Mis generated by combining a discrete set of flapping frequencies with a discrete set of tail angles, i.e., M=D×F={(δ, f)|δ∈D∧f∈F}. The initial state is inserted as the tree’s root. Then, at each iteration, all leaf states are expanded with all possible maneuvers. Given a state si, a flight maneuver defined by tail angle δand frequency f, and a time step ts, the function ODE(si, δ, f, ts)integrates the equations 7 in Section 4 for time tsto produce a flight state that becomes a new node of the tree. The states that are eventually inserted into the tree are decided according to two pruning procedures that will be detailed in Section 5.1. Algorithm 1: OSPA Input : (s0, sf, M, ts, kd, kw) Output: τ∗ 1tree ←Tree() 2tree.root ←s0 3leaves ←GetLeaves(tree) 4corridor ←GetCorridor(s0,sf) 5while leaves.length > 0do 6states ←List() 7for siin leaves do 8for (δ, f)in Mdo 9s0←ODE(si, δ, f, ts) 10 if GetDist(corridor, s0)≤kdand s0.x ≤sf.x then 11 s0.parent ←si 12 states.Add(s0) 13 end 14 end 15 partitions ←GetPartitions(states,kw) 16 for cin partitions do 17 s∗←GetOptimalState(c) 18 si←s∗.parent 19 si.AddChild(s∗) 20 end 21 leaves ←GetLeaves(tree) 22 end 23 τ∗←SearchOptimalPath(tree, sf) 24 return τ∗ We assume that the ornithopter has forward motion in the XZ plane, so the method generates states with increasing X-axis values between time steps. The tree computation ends when all reached states have greater X-axis value than the final state sf. In that case, the best sequence of maneuvers is returned and the algorithm terminates. This is done by the function SearchOptimalPath(), that computes the tree path τ∗with minimum energy consumption. The last node of the solution τ∗is chosen as the one with lowest energy consumption among those within a tolerance distance to sfthat is set by the user. OSPA searches for optimal trajectories in terms of energy, and hence, whenever a new node is added, the algorithm computes and stores the accumulated energy at that node. In order to model the energy consumed by the ornithopter performing a certain maneuver for a time step ts, we use the following formula: E=ts(Kaerof3+cr).(11) The first term represents the dominant energy consumption, which is due to flapping wings. Using equations in Section 4 with some simplifications 3, it can be proven that this consumption is proportional to the cube of the flapping frequency, with a constant coefficient Kaero that depends on several physical characteristics of the ornithopter, such as the wings’ profile, their inertia and their movement amplitude. However, as modelling all those effects precisely is complicated, we opted for estimating the value of Kaero empirically 4. The second term models the residual energy consumption crwhen the ornithopter is not flapping, mainly due to the onboard electronics. During gliding, we measured empirically for our ornithopter 3Due to space limitations, we do not reproduce here the tedious proof. 4All the experiments in this paper used a value Kaero = 2.5W/Hz3, obtained empirically for our ornithopter prototype. 8 that the cost of moving the tail was negligible compared to the average electronics consumption. Therefore, we consider the cost crconstant 5. As expected, it is important to note that Equation 11 indicates that the main energy consumption is produced by flapping maneuvers. For gliding, the energy efficiency is related with the temporal length of the maneuver. Let us now analyze the size of the tree Tgenerated by our method. If we have |M|=|D| × |F|=m different maneuvers that can be selected at each iteration, the whole tree construction without pruning operations takes O(hrm) time, where his the tree height 6and ris the average time needed by the integrator ODE(). Note that the height hdepends on the time step ts; the smaller ts, the larger the tree will be to reach the final state. Our OSPA algorithm can achieve energy-efficient trajectories if we take values of the time step short enough and use enough number of maneuvers. However, the computational complexity increases exponentially with the number of maneuvers, and a more reduced set of states may be enough to achieve competitive approximate solutions to the final state. Therefore, in the next section we propose two pruning procedures which alleviate the computational cost of the algorithm and yield an efficient planner for short and medium distance flights. 5.1 Tree reduction OSPA includes two specific procedures to reduce the tree size and speed up the algorithm. First, we reduce the original tree Tto a pruned tree T0that only keeps nodes whose position is close to a hypothetical optimal trajectory P∗. This pruning operation relies on the idea that the tree T0will produce optimal solutions similar to those in T, as long as the pruned nodes are not in the vicinity of P∗. In principle, this optimal trajectory is unknown, but we propose a parametric curve to estimate P∗that acts as guide in the tree pruning. Second, considering many states that are very close in the tree may lead to redundant calculation of similar trajectories. This fact motivates our second procedure to reduce further the tree size, where we obtain a new tree T00 by creating partitions with the nodes in T0and keeping only a witness node for each partition. Note that our pruning procedures assume that there is an optimal trajectory P∗which has robust clearance, that is, the nodes in T00 are enough to compute a near optimal trajectory. We support this assumption experimentally in Section 6. Figure 4 shows an example of an original tree Tand the effect of applying our pruning operations to obtain the trees T0and T00. Note that, some of the leaves in T00 are still irrelevant flight states for a suitable solution since they are far away from the final state. This is why OSPA only considers for last state selection those within a desired tolerance distance to the target. In the following, we elaborate on our two pruning operations to perform the incremental construction of T00. 5.1.1 First pruning operation: the corridor The first procedure to prune the tree consists of imposing physical constraints on the admissible trajectories. Thus, we speed up operations in OSPA but also avoid pathological states that would be unlikely. More precisely, we define a corridor region Cconnecting the initial and final states and only consider tree nodes within that corridor, discarding those out of the corridor. The key idea is building Cin such a manner that the (unknown) optimal trajectory P∗is likely to lie within that corridor. We generate the corridor Cas follows. First, we take a parametric curve on the XZ plane that connects the initial and final states, and adjust its parameters so that the curve is likely to resemble the optimal trajectory P∗. Let this reference curve be denoted as c P∗, then Cis defined as the region of points in the XZ plane whose minimum Euclidean distance to c P∗is not greater than kd. Thus, the tree T0results from pruning all nodes in the original Tthat are out of C. Figure 4b illustrates an example for a line segment c P∗connecting the initial and final states. Moreover, the functions GetCorridor() and GetDist() in Algorithm 1 compute the corridor Cand the minimum Euclidean distance of a flight state to c P∗, respectively. Recall that the actual optimal P∗is unknown, but we need a fairly reasonable estimation c P∗so that OSPA finds solutions that are close to the optimum. An option to learn those curves would be a totally bio-inspired approach, i.e., observing bird flights in order to copy the type of curves they follow; as they are assumed to be energy-efficient. This would require gathering large datasets which are not easy to obtain 5We estimated empirically an upper bound of cr= 5W. 6The height of Tis the maximum distance (number of edges) from the root to any node in T. 9 0 50 100 150 200 120 100 80 60 40 20 0 x(m) z(m) (a) AO-RRT 0 50 100 150 200 60 50 40 30 20 10 0 x(m) z(m) (b) OSPA Figure 7: Sampled waypoints in the tree (grey) and solution (blue) for an example experiment. The square denotes the tolerance region around the target state. states. The strategy consists of generating states by integrating numerically the nonlinear system dynamics using control inputs randomly selected. The algorithm stops when a computation time limit is reached and takes as solution a path toward the target state that minimizes a given objective function. In particular, we adapted the original AO-RRT 8to use our ornithopter model and the energy consumption as cost function. Moreover, we included a modification to sample control inputs from a discrete set. Otherwise, if we let the algorithm selects frequency values uniformly within the interval [0,6] Hz, the theoretical probability to pick f= 0 is zero; which would preclude us from performing gliding maneuvers, increasing significantly energy consumption. Therefore, we made the algorithm select control frequencies and tail angles randomly from the discrete set M. We created a set of 114 simulated scenarios to compare the two approaches. For each simulation, we took (x0, z0) = (0,0) as the position of the initial state and selected a target state uniformly within the intervals x∈[200,250] mand z∈[−90,20] m. Regarding target tolerance, we defined a square of 6-meter side centered in the chosen target point as the region to consider the goal as reached. The parameters selected for OSPA were ts= 12 s,kd= 10 m,kw= 15 (to achieve computation times below 200 s) and Mras the set of maneuvers. For AO-RRT, we fixed the maximum computation time to 200 s; and we checked that OSPA was able to run all the simulated scenarios with average time lower than 200 s. Another relevant parameter for AO-RRT was the time step between each new control action, as we realized it affected results in several manners. Therefore, we tested different values in our comparison to make it fairer. We used two metrics to compare the approaches: the average energy consumption of the solution trajectory over all scenarios; and the precision rate, which is the percentage of cases where a feasible solution was found within the 6-squared-meter tolerance region. Figure 7 shows the output of one of the simulated experiments for AO-RRT and OSPA, including the obtained solutions and the sampled waypoints in the trees. The figure depicts the advantages of the search strategy in OSPA; AO-RRT samples random states more uniformly distributed in the space, while in OSPA, thanks to the reference curve, states are more concentrated around the optimal solution. Figure 8 depicts the resulting metrics for the complete comparison. It can be seen that the precision rate of OSPA is always greater. Also, OSPA achieves more efficient trajectories in terms of energy. Thus, OSPA outperforms the state-of-the-art AO-RRT algorithm for ornithopter trajectory optimization. 7.2 A case study: landing for perching OSPA is thought for ornithopter navigation through short and medium distance flights, and it aims to optimize energy consumption during operation. For those scenarios, we can compute a trajectory online and then use nonlinear controllers at high rate for trajectory following. Now, we show a particular case study to demonstrate how OSPA can also be used for landing maneuvers when the ornithopter is going to perch. 8We used the open-source Python implementation provided by the authors. 16 AO-RRT(1) AO-RRT(3) AO-RRT(5) AO-RRT(7) AO-RRT(9) AO-RRT(12) OSPA(12) 0 1000 2000 3000 4000 E(W) 0 0.5 1 Energy Precision 0.25 0.75 Precision Figure 8: Comparison of energy and precision between OSPA and AO-RRT for different time step values (in parentheses). ξ δ (◦)f(Hz) 0.079 −1 0 0.107 −2 0 0.122 −3 0 0.081 −4 0 0.090 −5 0 0.093 −6 0 0.115 0 4 0.057 0 5 0.033 0 6 Table 8: Maneuvers used for perching planning. This set Mpwas obtained with a threshold ξ∗= 0.03. A perching maneuver was studied analytically in [8], consisting of two phases: a gliding phase (almost horizontal in this case) and a rapid pitch up to a high angle of attack. The example in Figure 3 shows various stages of this type of maneuvers. Perching maneuvers entail computing trajectories quite fast, as the ornithopter has no much time for reaction. Also, it is more relevant to land closer to the target rather than reducing the energy cost, as perching requires of high accuracy, particularly if the available area for landing is small. Therefore, we measure in this experiment the Euclidean distance to the target state considering only position, as vehicle velocities and attitude can be sensitive and may be adjusted by lower-level attitude controllers at high rate. We created a set of experiments to test OSPA computing trajectories to a landing spot. Typically, the initial and target positions should be relatively close and the altitude be descendant. Therefore, we set the ornithopter initially at the origin of coordinates with zero pitch and the target point at a 10-meter longitudinal distance with zcoordinate ranging between 2 and 5 m. Recall that positive zvalues mean descending. Since we aim at lower computation time when planning perching trajectories, we refine the set of maneuvers to apply OSPA. We experimentally selected a new reduced set of maneuvers for perching Mp, focusing specially on those with particular interest for that operation. To do this, we ran our set of experiments and selected Mpfollowing the same procedure as in Section 6.3. In particular, we used a probability threshold ξ∗= 0.03 and obtained the reduced set Mpshown in Table 8. It can be seen that most of these maneuvers involve gliding as it was demonstrated in [6] for optimized perching maneuvers. We set OSPA parameters to ts= 1 s,kd= 2 mand kw= 4. Table 9 shows results for the experiments performed. It is important to remark that, in all cases, the planning time is around 1 sand the distance error with respect to the target below 0.05 m. Interestingly, the optimized strategy yielded by OSPA was similar to the profile showed in Figure 3, i.e., a gliding phase followed by a pitch up with maximum upward 17 zf(m) Error (m) Time (s) 2 0.049 1.09 2.5 0.005 1.08 3 0.021 0.98 3.5 0.008 1.10 4 0.042 1.08 4.5 0.017 1.11 5 0.033 0.95 Table 9: Results for perching experiments using OSPA. The final point is located 10 meters away in longitudinal distance, and at different altitudes zf. Distance error to the target position and computational time are shown. elevator deflection. This indicates that OSPA can be a valid approach in practice for computing trajectories to approach a landing area where the ornithopter decides to perch. 8 Discussion and Future Work This work proposed OSPA, a new algorithm for kinodynamic planning of trajectories for autonomous ornithopters. The method is able to compute energy-efficient trajectories in an online fashion, combining gliding and flapping maneuvers. OSPA builds trees dynamically feasible and runs heuristic search to plan trajectories. This paradigm can be applied to any dynamic model (we used a nonlinear aerodynamic model for ornithopters) and different flight types. We demonstrated a proper performance of the algorithm for medium distance flights of up to 250 m, but also for planning short landing trajectories of up to 10 m. Computation time is suitable for online trajectory planning, achieving solutions for short flights in less than 1 s. Moreover, our experimental results showed that OSPA outperforms alternative probabilistic kinodynamic planners both in cost (total energy) and accuracy (distance to the target). An open-source implementation of OSPA and our benchmarks is available online 9. Notice that our current implementation is written in Python, so there is still room for improvement with more efficient languages like C. As future work, we mention some potential extensions for OSPA that we discuss in the following. Improving heuristic performance: One of the main aspects that affects the performance of the heuristic search in OSPA is the reference curve, as the algorithm relies on having a good approximation of optimal curves to guide search. We proposed curves computed empirically, but an open mathematical problem is to calculate the theoretical reference curve that minimizes the energy consumption for a given model. With a better estimation of that optimal energy curve, the parameters kdand kwcould be reduced to compute pseudo-optimal trajectories more efficiently. Planning in dynamic scenarios: OSPA can be used for trajectory planning in dynamic scenarios, recomputing trajectories online as the environment changes. Nonetheless, another problem to explore would be to address scenarios with static or mobile obstacles, not only open spaces. This could be done by combining state-of-the-art collision avoidance algorithms with OSPA, or integrating some procedure for collision checking within the planner. Planning in 3D: In this paper, we considered 2D trajectories for the ornithopter. However, OSPA is not limited to that, as ornithopter models in 3D could be also used with OSPA to plan 3D trajectories. This is particularly useful for settings where wind effects on lateral displacement cannot be neglected, and it could be tackled by considering curvature-constrained trajectories as in [1, 36]. Machine learning methods: These days methods for trajectory planning based on machine learning are spreading fast. One of the issues is the lack of data of real bird flights in order to imitate their trajectories with ornithopters. In this sense, OSPA may be helpful to generate artificial bio-inspired datasets with pseudo-optimal trajectories, which could be used for training alternative machine learning methods. Experiments with real ornithopter: Finally, we plan to use OSPA to compute trajectories on board our real ornithopter prototype within the framework of the GRIFFIN project. For that, we will combine the 9https://github.com/fragnarxx/kinodynamic-planning 18 planner with nonlinear controllers for flight stabilization and trajectory following. References [1] W. H. Al-Sabban, L. F. Gonzalez, and R. N. Smith. Wind-energy based path planning for unmanned aerial vehicles using markov decision processes. In 2013 IEEE International Conference on Robotics and Automation, pages 784–789. IEEE, 2013. [2] V. Arabagi, L. Hines, and M. Sitti. Design and manufacturing of a controllable miniature flapping wing robotic platform. The International Journal of Robotics Research, 31(6):785–800, 2012. [3] F. Ayancik, Q. Zhong, D. B. Quinn, A. Brandes, H. Bart-Smith, and Keith W. Moored. Scaling laws for the propulsive performance of three-dimensional pitching propulsors. Journal of Fluid Mechanics, 871:1117–1138, 2019. [4] J. T. Betts. Practical methods for optimal control and estimation using nonlinear programming, volume 19. Siam, 2010. [5] D. Brescianini and R. D’Andrea. Computationally efficient trajectory generation for fully actuated multirotor vehicles. IEEE Transactions on Robotics, 34(3):555–571, June 2018. [6] R. Cory and R. Tedrake. Experiments in fixed-wing uav perching. In AIAA Guidance, Navigation and Control Conference and Exhibit, page 7256, 2008. [7] W. P. Coutinho, M. Battarra, and J. Fliege. The unmanned aerial vehicle routing and trajectory optimisation problem, a taxonomic review. Computers & Industrial Engineering, 120:116–128, 2018. [8] W.J. Crowther. Perched landing and takeoff for fixed wing uavs. In NATO symposium on unmanned vehicles for aerial, ground, and naval military operations, pages 9–13, 2000. [9] G.C.H.E. de Croon, K.M.E. de Clercq, R. Ruijsink, B. Remes, and C. de Wagter. Design, aerodynamics, and vision-based control of the delfly. International Journal of Micro Air Vehicles, 1(2):71–97, 2009. [10] J. M. Dietl and E. Garcia. Ornithopter optimal trajectory control. Aerospace Science and Technology, 26(1):192 – 199, 2013. [11] R. Fernandez-Feria. Linearized propulsion theory of flapping airfoils revisited. Physical Review Fluids, 1(8):084502, 2016. [12] R. Fernandez-Feria. Note on optimum propulsion of heaving and pitching airfoils from linear potential theory. Journal of Fluid Mechanics, 826:781–796, 2017. [13] I. E. Garrick. Propulsion of a flapping and oscillating airfoil. Technical Report TR 567, NACA, 1936. [14] J. Grauer and J. Hubbard. Modeling of ornithopter flight dynamics for state estimation and control. In Proceedings of the American Control Conference, pages 524–529, June 2010. [15] K. Hauser and Y. Zhou. Asymptotically optimal planning by feasible kinodynamic planning in a statecost space. IEEE Transactions on Robotics, 32(6):1431–1443, December 2016. [16] J. Hoff, J. Syed, A. Ramezani, and S. Hutchinson. Trajectory planning for a bat-like flapping wing robot. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 6800–6805, November 2019. [17] S. Karaman and E. Frazzoli. Optimal kinodynamic motion planning using incremental sampling-based methods. In IEEE Conference on Decision and Control (CDC), pages 7681–7687, Dec 2010. [18] S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7):846–894, 2011. 19 [19] Y. Li, Z. Littlefield, and K. E. Bekris. Asymptotically optimal sampling-based kinodynamic planning. The International Journal of Robotics Research, 35(5):528–564, 2016. [20] Y. Li, Z. Littlefield, and K. E. Bekris. Asymptotically optimal sampling-based kinodynamic planning. The International Journal of Robotics Research, 35(5):528–564, 2016. [21] A. Mart´ın-Alc´antara, P. Grau, R. Fernandez-Feria, and A. Ollero. A simple model for gliding and low-amplitude flapping flight of a bio-inspired uav. In International Conference on Unmanned Aircraft Systems (ICUAS), pages 729–737, June 2019. [22] D. Mellinger and V. Kumar. Minimum snap trajectory generation and control for quadrotors. In IEEE International Conference on Robotics and Automation (ICRA), pages 2520–2525, May 2011. [23] A. A. Menezes and P. T. Kabamba. Efficient and resilient micro air vehicle flapping wing gait evolution for hover and trajectory control. Engineering Applications of Artificial Intelligence, 54:1 – 16, 2016. [24] M. W. Mueller, M. Hehn, and R. D’Andrea. A computationally efficient motion primitive for quadrocopter trajectory generation. IEEE Transactions on Robotics, 31(6):1294–1310, Dec 2015. [25] Q.-V. Nguyen and W. L. Chan. Development and flight performance of a biologically-inspired tailless flapping-wing micro air vehicle with wing stroke plane modulation. Bioinspiration & Biomimetics, 14(1):016015, December 2018. [26] H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, and E. Galceran. Continuous-time trajectory optimization for online uav replanning. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 5332–5339, October 2016. [27] A. A. Paranjape, S. Chung, and J. Kim. Novel dihedral-based control of flapping-wing aircraft with application to perching. IEEE Transactions on Robotics, 29(5):1071–1084, October 2013. [28] A. A. Paranjape, M. R. Dorothy, S.-J. Chung, and K.-D. Lee. A flight mechanics-centric review of bird-scale flapping flight. International Journal of Aeronautical and Space Sciences, 13(3):267–281, 2012. [29] A. A. Paranjape, K. C. Meier, X. Shi, S.-J. Chung, and S. Hutchinson. Motion primitives and 3d path planning for fast flight through a forest. The International Journal of Robotics Research, 34(3):357–377, 2015. [30] M. Posa, C. Cantu, and R. Tedrake. A direct method for trajectory optimization of rigid bodies through contact. The International Journal of Robotics Research, 33(1):69–81, 2014. [31] S. Stoneman and R. Lampariello. Embedding nonlinear optimization in RRT* for optimal kinodynamic planning. In IEEE Conference on Decision and Control, pages 3737–3744, December 2014. [32] T. Theodorsen. General theory of aerodynamic instability and the mechanism of flutter. Technical Report TR 496, NACA, 1935. [33] A. L. Thomas. On the aerodynamics of birds’ tails. Philosophical Transactions of the Royal Society of London. Series B: Biological Sciences, 340(1294):361–380, 1993. [34] J. Wang, M. J. Zahr, and P.-O. Persson. Energetically optimal flapping flight via a fully discrete adjoint method with explicit treatment of flapping frequency. In AIAA Computational Fluid Dynamics Conference, 2017. [35] D. J. Webb and J. van den Berg. Kinodynamic RRT*: Asymptotically optimal motion planning for robots with linear dynamics. In IEEE International Conference on Robotics and Automation (ICRA), pages 5054–5061, May 2013. [36] X. Zhang, J. Chen, B. Xin, and Z. Peng. A memetic algorithm for path planning of curvature-constrained UAVs performing surveillance of multiple ground targets. Chinese Journal of Aeronautics, 27(3):622– 633, 2014. 20