Bryan Penin
email: bryan.penin@inria.fr

Paolo Robuffo

Franc ¸ois Chaumette
email: francois.chaumette@inria.fr

F Chaumette

P Robuffo Giordano

Minimum-Time Trajectory Planning Under Intermittent Measurements

Keywords: Reactive and Sensor-Based Planning, Motion and Path Planning, Aerial Systems: Perception and Autonomy

This paper focuses on finding robust paths for a robotic system by taking into account the state uncertainty and the probability of collision. We are interested in dealing with intermittent exteroceptive measurements (e.g., collected from vision). We assume that these cues provide reliable measurements that will update a state estimation algorithm wherever they are available. The planner has to manage two tasks: reaching the goal in a minimum time and collecting sufficient measurements to reach the goal state with a given confidence level. We present a robust perception-aware bi-directional A* planner for differentially flat systems such as a unicycle and a quadrotor UAV and use a derivative-free Kalman filter to approximate the belief dynamics in the flat space. We also propose an efficient way of ensuring continuity and feasibility by exploiting the convex-hull property of B-spline curves.

I. INTRODUCTION

U NCERTAINTY-AWARE planning, also called belief- space planning, has received considerable attention in recent years. Indeed, model, sensors and environment uncertainties are inherent to many robotic applications and may lead to a failure of the task or impair the possibility to accurately follow a path if disregarded at the planning stage. A class of control techniques that operate over the belief space, known as partially-observable Markov decision processes (POMDPs) [START_REF] Thrun | Probabilistic robotics[END_REF] has been derived to address the above problem. Another class of works exploits local optimal control policies assuming a linear quadratic Gaussian (LQG) control strategy. However, these approaches suffer from the "curse of dimensionality". In particular POMDPs are notorious for their computational complexity that may prohibit their application for navigation in complex or uncertain environments in high dimensional state spaces. In [START_REF] Rafieisakhaei | Belief space planning simplified: Trajectory-optimized lqg (t-lqg)[END_REF] a more scalable LQG variant is proposed and applied to environments with discontinuous sensing regions. An approximate solution to POMDPs is given in [START_REF] Bry | Rapidly-exploring random belief trees for motion planning under uncertainty[END_REF] but with the use of considerable pre-processing. To deal with more complex objectives, deterministic planners such as RRT* and A* are now very popular since they benefit from asymptotic optimality and explore the whole configuration space efficiently. In [START_REF] Gonzalez | Planning with uncertainty in position an optimal and efficient planner[END_REF] a graph-search based on A* is proposed by discretizing the environment into cells for finding a safe route for a unicycle vehicle. Active perception with a quadrotor has been addressed in [START_REF] Costante | Perception-aware path planning[END_REF] for determining the path with minimal state uncertainty, and in [START_REF] Davis | C-opt: Coverage-aware trajectory optimization under uncertainty[END_REF] for maximizing coverage in presence of obstacles, localization and sensing uncertainty. Recently, [START_REF] Van Den | Motion planning under uncertainty using differential dynamic programming in belief space[END_REF] proposed an approximate POMDP control policy based on an initial guess trajectory returned by a RRT planner in a discretized environment. Fig. 1: Simulation environment for our framework. An optimal collision-free trajectory considering a unicycle connects the initial state (green dot) and a final state (yellow dot) in presence of obstacles (blue boxes). The pose uncertainty is represented by the blue ellipsoids whose size is reduced as soon as a landmark (red bars) is close enough to the robot and enters the field of view of the camera attached to the robot. We assume the landmarks are not occluded by the obstacles. The propagated edges of the two graphs are rendered as the red curves.

In this work, we aim at planning a trajectory from an initial to a final state in presence of obstacles and input constraints for non-trivial robotic systems (like a quadrotor). We assume that the state is not available (especially the position) but on-board sensors (including a camera) are used to reconstruct the state with some estimation algorithm fusing position measurements reconstructed from vision. Note that these measurements may be temporarily unavailable because of limited field of view, maximum range and so on. We want that the path guarantees some desired level of uncertainty in the reconstructed state despite the possible non-availability of the measurements. More precisely, the goal state has to be reached with a bounded state uncertainty to guarantee some confidence level on the robot location. Therefore, the system has to collect sufficient information from visual landmarks sparsely placed in the environment to satisfy this final constraint (see Fig. 1 considering a unicycle equipped with a front-looking camera with reference to Fig. 2). Fig. 2: A unicycle equipped with a fixed camera receives position measurements updates whenever a visual landmark (red dot) is close enough and enters the field of view.

Literature in perception-aware planning has generally focused on maximizing observability (or minimizing the state uncertainty) based on some criteria e.g., the trace or the smallest eigenvalue of the covariance matrix. This strategy definitely helps in finding a path that tries to collect as much information as possible for preventing the state uncertainty to increase too much. However, the path itself may be severely suboptimal in terms of length and duration (e.g., [START_REF] Hausman | Observabilityaware trajectory optimization for self-calibration with application to uavs[END_REF] for optimal selfcalibration of UAVs). Indeed, the path length is generally not constrained and can be excessively long, especially if the robot needs to pass by all the regions/landmarks with richest information.

In this work, we propose a minimum-time planning algorithm for dynamic systems returning feasible and robust trajectories that do not guarantee minimal state uncertainty along the trajectory but a bounded state uncertainty with given bounds at the goal, which we consider as a more practical application.

II. RELATED WORKS A. Planning in the belief space of dynamical systems

An admissible position uncertainty is defined in [START_REF] Pepy | Reliable robust path planning with application to mobile robots[END_REF] as a goal area of a given size that a nonholonomic robot has to enter. A RRT variant is implemented where uncertain states are modeled as boxes. Our work is mostly based on the recent work of [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF], [START_REF] Liu | Search-based Motion Planning for Aggressive Flight in SE (3)[END_REF] that propose an efficient A* planner in the flat space of a quadrotor. It is applied to aggressive manoeuvres and precise collision avoidance that are function of the robot attitude. A variant of RRT for flat systems is detailed in [START_REF] Bascetta | Flat-rrt*: A sampling-based optimal trajectory planner for differentially flat vehicles with constrained dynamics[END_REF] to produce smooth dynamically feasible motion plans in real-time and in [START_REF] Pivtoraiko | Incremental micro-uav motion replanning for exploring unknown environments[END_REF] for online navigation in dynamic environments with a quadrotor. Both works exploit differential flatness to build a look-up table of pre-computed feasible motion primitives.

B. Contributions

In a previous work [START_REF] Penin | Vision-based reactive planning for aggressive target tracking while avoiding collisions and occlusions[END_REF] we considered hard visibility constraints that may become too restrictive for minimum-time planning. In this work, we propose to relax these constraints by allowing temporary visual cues losses for performing faster trajectories in larger and more complex environments. We implement a bi-directional A* algorithm that grows two graphs, one from the initial state and one from the final state. A solution trajectory is built by connecting the two graphs. The work presented in this paper blends the following features within a graph-search algorithm: (i) incorporation of model and sensor uncertainty in collision avoidance and perception, (ii) generation of minimum-time and feasible trajectories for flat dynamic systems, (iii) incorporation of discontinuous visual measurements, (iv) efficient graph connection using the convex-hull property of B-spline curves.

In contrast to [START_REF] Liu | Search-based Motion Planning for Aggressive Flight in SE (3)[END_REF] we include perception constraints and state uncertainty and directly minimize the time. To the best of our knowledge, this is the first time minimum-time trajectories are generated in a graph-search planner while accounting for uncertainty in the visual perception which is affected by the system state.

The rest of this paper is organized as follows. Sect. III introduces differential flatness and the modelling of a quadrotor. Sect. IV presents the uncertainty-aware planner formulated as a graph-search problem. How the graph is built is described in Sect. V. The graphs rewiring is detailed in Sect. VI. In Sect. VII simulation and experimental results are presented for a quadrotor with an onboard camera. Finally we draw some conclusions and future directions in Sect. VIII.

III. PRELIMINARIES

A. Differential flatness

Differentially flat systems are systems whose state χ and inputs u can be expressed as algebraic functions of flat outputs derivatives up to some suitable order [START_REF] Fliess | Flatness and defect of nonlinear systems: Introductory theory and examples[END_REF]. Differential flatness is often used for planning purposes: i) the problem size is reduced, ii) any smooth enough curve in the flat space is feasible by the real system, iii) the system dynamics are linear in the flat space. The proposed algorithm is applicable to systems represented as d independent chains of integrators of a given order r of the form

η prq " ν (1)
where ν P R d denotes the new inputs and η P R d are the flat outputs. Let us define the system state in the flat space as s " ´η, ...,

η pr´1q ¯P R dpr´1q (2)
In the rest of the paper we consider the system position as the flat outputs, e.g., η " px, y, zq in the three-dimensional space.

B. Application to a quadrotor UAV

With reference to Fig. 3 the quadrotor state is defined as χ " pp, R, v, ωq P SEp3q ˆR6 where p P R 3 is the position of the robot COM in the world frame W " te 1 , e 2 , e 3 u (being e i the i-th column of the identity matrix), R P SOp3q is the rotation matrix from W to the body frame B " tx B , y B , z B u, v the COM linear velocity expressed in W and ω the angular velocity expressed in B. The quadrotor is known to be flat with Fig. 3: Quadrotor model flat outputs px, y, z, ψq [START_REF] Faessler | Differential flatness of quadrotor dynamics subject to rotor drag for accurate tracking of highspeed trajectories[END_REF]. The quadrotor dynamics can be decoupled into four linear subsystems of the form

x p4q " u 1 , y p4q " u 2 , z p4q " u 3 , ψ p2q " u 4 ( 3
)
where ν " pu 1 , u 2 , u 3 , u 4 q defines the new control inputs in the flat space. For the sake of simplicity, we do not plan over the yaw angle ψ that is assumed to be constant at zero. Moreover, we consider the quadrotor as three tripleintegrators controlled in jerk along axes X, Y and Z (i.e., η " px, y, zq P R 3 ). With the above simplifications we seek to alleviate the planner whose complexity grows exponentially with the state dimension. Finally, we consider the state vector s " pη, 9 η, : ηq and inputs ν " η p3q . We consider that the quadrotor is equipped with a fixed downward-looking camera capable of providing reliable position measurements when fixed landmarks on the ground enter the limited field of view.

IV. PROBLEM FORMULATION

We aim at solving an optimal control problem connecting an initial state s init and a final state s goal in a minimum time T . us define the following optimal control problem.

Problem 1. Find the input ν and time T such that: min ν,T T s.t. sp0q " s init , spT q " s goal , maxteigpP η pT qqu ď λ, p 9 ηpτ q, : ηpτ q, η p3q pτ qq P X f ree @τ P r0, T s

where eigpP η pT qq P R d contains the eigenvalues of the position covariance matrix P η at the goal state s goal and X f ree :" r´v, vs 3 ˆr´ā, ās 3 ˆr´j, js 3 denotes the hypercube space of the admissible velocities, accelerations and jerks.

The desired bound on the position uncertainty is defined by λ ą 0. For nonlinear systems such as a quadrotor, the Extended Kalman Filter (EKF) is often used for approximating the belief dynamics. The EKF is based on a linearization of the system dynamics which results in cumulative errors due to the local linearization assumption. In this paper, since we plan directly in the flat space we can use a so-called derivative-free Kalman filter without the need for derivatives and Jacobians calculations. Moreover, the state estimation accuracy of a derivative-free Kalman filter can be improved w.r.t. a standard EKF, especially for nonlinear systems [START_REF] Rigatos | Derivative-free kalman filtering for autonomous navigation of unmanned ground vehicles[END_REF]. Considering the linear equivalent system in the flat space one defines the process model. When a landmark is visible we have

9 s " As `Bu `ζ, y " Cs ` ( 5
)
where ζ is the process noise and is the measurement noise. For a flat system controlled in acceleration (i.e., r " 2), assuming the velocity is estimated through filtering of position measurements, the matrices A, B, and C are given by

A " ¨0 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 ‹ ‹ ' , B " ¨0 0 0 0 1 0 0 1 ‹ ‹ ' , C " ˆ1 0 0 0 0 1 0 0 ˙(6)
In the next sections we will show how Problem 1 can be transformed from an infinite dimensional optimal control problem to a finite dimensional bi-directional graph-search problem.

We choose to extend two graphs to improve the search, to increase the rate of convergence and the chance to find a solution especially in complex and cluttered environments. Moreover, the use of two graphs generally propagates fewer vertices than with a single graph.

A. Motion primitives

As in [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF] we use polynomials to parameterize the flat state components and generate motion primitives to explore the flat space in a discrete way (see [START_REF] Thrun | Probabilistic robotics[END_REF]). More precisely, by applying a number of sampled constant inputs ν k P r´j, ..., js 3 along each axis for a duration τ ą 0 one can iteratively build a graph GpV, Eq rooted in state s 0 . Here, V defines the set of discrete states denoted as the vertices s in the graph representation that are connected with a motion primitive referred as an edge in the set E (e.g., see Fig. 1). For a flat system controlled in jerk (i.e,. r " 3) a motion primitive represents the state sptq starting at state s 0 for t P r0, τ s with a curve defined as sptq " M pν k , s 0 , tq :" » -

ν k t 3 6 `: η 0 t 2 2 `9 η 0 t `η0 ν k t 2
2 `: η 0 t `9 η 0 ν k t `: η 0 fi fl [START_REF] Van Den | Motion planning under uncertainty using differential dynamic programming in belief space[END_REF] These trajectories reflect the system dynamics thanks to differential flatness and provide the minimum jerk between the states s 0 and spτ q [START_REF] Mueller | A computationally efficient motion primitive for quadrocopter trajectory generation[END_REF]. The free flat space will be explored with a propagation of these motion primitives further detailed in Sect. V. Naturally, changing the admissible bounds for ν k and duration τ will affect the free space coverage.

Problem 1 can be reformulated as Problem 2 in the graph representation where we seek the trajectory connecting the initial and goal states with the optimal control sequence ν k and the minimal number N ˚of motion primitives. Problem 2. Find the sequence ν k and N such that:

min ν k ,N N s.t. s 0 " s init , s N " s goal , maxteigpP η N qu ď λ, p 9 η k , : η k , η p3q k q P X f ree @k P v0, N w (8a) (8b) (8c) (8d)
where P η N is the covariance matrix on the position at the goal vertex. The resulting trajectory will have a total time N ˚τ . Finally, collisions are avoided by considering the robot shape as representative of the position uncertainty ellipse (or ellipsoid in 3D) whose estimation is detailed in the next section. Motion primitives that violate the collision constraints are not added to the graph.

In the next section we will show how state uncertainty is included in visual perception and in collision avoidance to guarantee perception of visual measurements and safe navigation to a given level of confidence.

B. State estimation uncertainty

Let σ be the major axis of the uncertainty ellipse P η at a given state. Then, for a 99% confidence level one has σ 99% " ?

9.21 ? λ (from the Chi-Square probabilities) where λ is the largest eigenvalue of P η . This confidence ellipse defines the region that contains 99% of all samples that can be drawn from the Gaussian distribution. We take a sphere with radius σ 99% as representative of the robot occupancy. It will vary with the pose uncertainty and will be included in the planner for ensuring robust collision-free paths. Now, let us consider state uncertainty in the visual perception. With reference to Fig. 4, in order to check that a landmark at position r L is visible along an edge, we impose conditions on angles β 1 and β 2 on both planes X-Z and Y-Z. On a given plane one has

β i " arccos ˆpp ´rL q.l i }p ´rL } ˙, i " 1, 2 (9)
where l i are given by

l 1 " ˆcos α ´sin α sin α cos α ˙t, l 2 " ˆcos α sin α ´sin α cos α ˙t (10)
With g as the (constant) gravity acceleration and a as the robot acceleration in the world frame one has t " pa `gq{}a `g}.

Since the β i are function of the position and the acceleration, the bearing uncertainty ∆β P R on a given plane can be computed as

∆β " ˆBβ Bη Bβ B : η ˙T ˆP η 0 0 P : η ˙ˆBβ Bη Bβ B : η ˙(11)
where P : η is the acceleration covariance matrix. The state covariance matrices are evaluated with the Kalman filter along each valid discretized motion primitive. This way one can ensure a (theoretical) 99% confidence on the perception if the following upper bound conditions are satisfied on both planes X-Z and Y-Z at a given state

|β i | `?9.21∆β ă 2α, i " 1, 2 (12)
These conditions allow an exact and fast evaluation of the visibility and only rely on the flat state. Moreover, it allows us to consider a realistic pyramid-shaped field of view (since we do not plan over the yaw). The update step of the Kalman filter is therefore applied with the simulated measurements whenever conditions [START_REF] Bascetta | Flat-rrt*: A sampling-based optimal trajectory planner for differentially flat vehicles with constrained dynamics[END_REF] are met along the propagated motion primitives.

V. BUILDING THE GRAPH

In this section we show how to exploit some vertices to efficiently explore the free space with the design of an heuristic function in order to build the graph. Traditionally, distancebased heuristics are used but they are not very relevant for Fig. 4: The quadrotor in the vertical planes X-Z and Y-Z with ψ " 0. Having uncertainties on the state affects the perception. To evaluate if a landmark (red blob) is visible under the state uncertainty we check that condition ( 12) is satisfied on both planes.

dynamic or nonholonomic systems that cannot change their velocity, acceleration or orientation instantaneously. That is why a heuristic function more appropriate for second-or higher-order systems has been proposed in [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF], by taking smoothness into account. As well know, A* algorithms rely on two functions: the heuristic function hps, s 1 q that encodes an (optimistic) approximation of the cost-to-go from a vertex s to a goal vertex s 1 and the function gpsq which represents the cost of vertex s. Without a heuristic, A* is equivalent to a Dijkstra search, but encoding some theoretic information into the heuristic function can greatly reduce the number of expansions in favouring exploration toward promising directions/areas. We use the heuristic function proposed in [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF] that originates from the resolution of Pontryagin's minimum principle and invite the reader to refer to the latter paper for more information. This function now encodes the "effort" required to connect two states given the considered control input (e.g., velocity, acceleration or jerk) and is used to select vertices leading to the exploration toward regions with minimal energy in order to encourage smooth trajectories. The cost of an edge itself is gpsq " τ because we want to minimize the time. We propose a bi-directional A* algorithm that builds two graphs G 1 and G 2 . G 1 starts at the initial state s 0 and G 2 starts at the final state s N . Both graphs will be propagated and connected to return full trajectories from s init to s goal . The planner is detailed in the following section.

Designing an efficient space exploration is tedious when complex tasks are involved and should not rely on a too strong a priori. Namely, in our case, the search should not be biased towards the goal since it may not collect sufficient visual cues from the landmarks to satisfy (8c). When multiple goals are present one may bias the search towards these goals, here, the landmarks. However, ensuring convergence to the final goal is not straightforward, especially for dynamic systems with perception goals. In the end, we choose to not rely on any exploration a priori to be able to deal with any environment and landmark configurations (provided a solution exists). We will rely instead on random-based exploration by smoothly propagating vertices toward states sampled randomly in the free space. Algorithm 1 runs for a given number of iterations I and is detailed below. Note that the uncertainty is only propagated on graph G 1 with the Kalman filter since graph G 2 is grown backwards (i.e., from the final goal s goal towards the initial state s init ). With reference to Fig. 5, the algorithm Algorithm 1 Bi-A* 1: gps 1 best q Ð 8, gps 2 best q Ð 8 2: for i Ð 1 to I do 3:

s rand Ð Samplepq 4: pX 1 near , X 2 near q Ð N earV erticesps rand , G 1 , G 2 , ρq 5: pX 1 c , X 2 c q Ð N earV erticesps rand , G 1 , G 2 , γ c q 6: if X 1 c ‰ Ø and X 2 c ‰ Ø then 7: ps 1 new , s 2 new q Ð ConnectGpX 1 c , X 2 c q 8: pP η N , collq Ð BackP ropps 2 new , P s 1 new , G 2 q 9:
if maxteigpP η N qu ď λ and !coll 10:

if gps 1 new q `gps 2 new q ă gps 1 best q `gps 2 best q 11:

s 1 best " s 1 new , s 2 best " s 2 new 12:
if X 1 near ‰ Ø then 13:

L s Ð GetSortedListpX 1 near q 14:

s 1 Ð ChooseBestP arentpL s , s N q 15: L c 1 Ð ExtendV ertexps 1 q 16: G 1 Ð InsertV erticespL c 1 q 17: if X 2 near ‰ Ø then 18: L s Ð GetSortedListpX 2 near q 19: s 2 Ð ChooseBestP arentpL s , s 0 q 20: L c 2 Ð ExtendV ertexps 2 q 21: G 2 Ð InsertV erticespL c 2 q 22:
if X 1 near " Ø and X 2 near " Ø then 23:

ps 1 , s 2 q Ð BestV erticespG 1 , G 2 q 24: L c 1 Ð ExtendV ertexps 1 q 25: G 1 Ð InsertV erticespL c 1 q 26: L c 2 Ð ExtendV ertexps 2 q 27:
G 2 Ð InsertV erticespL c 2 q return s 1 best , s 2 best procedures are detailed below: Sample: returns an independent and uniformly distributed random sample vertex s rand in the free space.

NearVertices: given a sample vertex s rand , a graph G " pV, Eq and a ball region B r of a given radius ρ, the set of near vertices is defined as N earps, G, ρq " ts P V : dps, s rand q ď ρu where d is the Euclidean distance and ρ " γplogpKq{Kq 1{q is the radius for expansion with K is the number of vertices and q is the space dimension. The ball radius helps capturing vertices when the graph is hollow and shrinks with the number of vertices to reduce the computation time. We use a constant radius γ c for finding connections candidates (see procedure ConnectG).

GetSortedList: given a list of vertices V and a goal s 1 , this function returns a list L s of the sorted vertices s P V in increasing heuristic cost hps, s 1 q.

ChooseBestParent: the vertex with lowest h cost from a list of vertices is chosen for expansion. We seek to find the parent vertex that will expand vertices towards the given goal with the lowest energy (highest smoothness).

BestVertices: when no near vertices are found in B r , this function finds the vertex s in graph G 1 with lowest cost hps, s N q and analogously for G 2 with hps, s 0 q.

ExtendVertex: propagates a motion primitive from a given parent vertex. This function includes the belief state propagation with the Kalman filter and collision and feasibility tests.

InsertVertices: valid vertices/edges are added to the graph and marked as children from their parent vertex.

InsertVertex: this function inserts a single vertex/edge pair.

ConnectG: this procedure is triggered whenever vertices from both graphs are found in the procedure NearVertices within a second ball region of constant radius γ c centered on s rand . Indeed, we seek pairs of vertices in a vicinity region to perform connection tests (see Algorithm 2) using the function solveQP presented in Sect. VI. Note that γ c can for instance, be chosen as the "spatial resolution" of motion primitives or larger to find more connections.

BackProp: given a vertex s 0 with a covariance matrix P 0 from graph G 1 , once a connection is found we back-propagate the state uncertainty through G 2 by considering the sampled states between s 0 and the goal s N (see Fig. 5).

Algorithm 1 aims at finding the most direct trajectory towards the goal, especially in case of low process noise and tries to mimic a couple of nice properties of classic graph-search planners, namely: i) expansion towards unexplored regions; ii) probabilistic completeness due to a uniform random walk; iii) asymptotic optimality.

Algorithm 2 ConnectG

Input: X 1 c , X 2 c 1: success " false 2: for s 1 in X 1 c do 3: for s 2 in X 2 c do 4:
if hps 1 , s 2 q ă h then 5:

success Ð solveQP ps 1 , s 2 q 6:

if success " true then

7:
return ps 1 , s 2 q 8: return 0

Algorithm 2 performs connection trials on the vertices in X 1 c , X 2 c if their heuristic cost is lower than a given value h. This value can be chosen off-line to skip connections that may require "too much" energy. Usually, graph-search planners for dynamic systems involve two steps. First, an optimal path is found ignoring the system dynamics. Then a refining step is performed by optimizing over a selection of state keyframes along the path. The resulting trajectory is smoothed and more adapted to dynamic systems (see e.g., [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF], [START_REF] Richter | Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments[END_REF]). However, the shape of this trajectory may strongly differ from the original path (e.g., in [START_REF] Richter | Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments[END_REF]). In our context, visual perception cannot be guaranteed with such a technique and it does not take into account the uncertainty in collision avoidance. A key role of the bi-directional planner used in this work is that if a connection is made, the initial and final states are exactly connected, which is generally not the case in the graph-search planners literature. For instance, [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF] stops the search when a vertex becomes close enough to the goal state s goal , a condition that may not be met if not properly tuned. In this work we aim instead at finding the optimal trajectory that will be directly tracked by the real system without additional refining steps. Fig. 5: 2D case: Inside a ball of radius ρ centered at s rand (black dot), picture 1 shows how the vertex with lowest cost h is chosen for expansion (in cyan). The black arrows represent the vertex velocity vector. If vertices from graph G 1 (in green) and from graph G 2 (in yellow) are found inside a ball region of fixed radius γ c , connections trials are performed except for connections with a high h cost (orange lines). Note that we consider the opposite velocity (and higher derivatives) vectors for vertices coming from graph G 2 . If a candidate connection is found (blue line) the uncertainty is propagated along G 2 starting from s 1 new (picture 3). If no collisions are found between the obstacles (blue box) and the uncertain robot occupancy (turquoise ellipses) and if the final constraint (8c) is satisfied on P η N , a solution trajectory is reconstructed from the initial vertex s 0 (green dot) to the goal vertex s N (yellow dot) and its total cost is evaluated.

Next section details how the connection between the two graphs is performed in an optimal and efficient way.

VI. CONNECTING THE GRAPHS

Connecting the two graphs is a critical step. One has to ensure state continuity between two candidate vertices s 1 P V 1 and s 2 P V 2 in a given time T c . This problem is known as the Boundary Value Problem (BVP). Moreover, one wants to ensure feasibility as well and connections have to be evaluated quickly since the process may be called many times. We propose an optimal formulation to the BVP that can be solved as a single convex quadratic program. We exploit the convexhull property of B-splines in order to impose constraints directly on the curve control points to alleviate the solver.

A. Solving the constrained BVP

The problem we want to solve is the following Problem 3. Find s, ν such that:

min s,ν ż Tc 0 }νpτ q} 2 dτ s.t. sp0q " s 1 , spT c q " s 2 ,
p 9 ηpτ q, ..., η prq pτ qq P X f ree @τ P r0, T c s

(13a) (13b) (13c)
We penalize the input norm to obtain a smooth connection trajectory. For the quadrotor one minimizes the jerk norm (i.e., r " 3). Now, we parameterize the flat state s as B-splines to turn the infinite dimensional problem to a finite one with a limited number of coefficients that can be solved numerically. A trajectory s is parameterized as sptq " i"n ÿ i"0 B i,p pτ qP , @τ P r0, T s [START_REF] Penin | Vision-based reactive planning for aggressive target tracking while avoiding collisions and occlusions[END_REF] where B i is a polynomial basis of degree p (of order k " p`1) and P P R n`1 represents the set of coefficients.

B. A linear quadratic program based on B-splines

The reason we use B-splines is for their convex-hull property that will allow us to write linear inequality constraints directly on the B-spline control points. A similar approach has been used in [START_REF] Van Loock | B-spline parameterized optimal motion trajectories for robotic systems with guaranteed constraint satisfaction[END_REF] for manipulators. This strategy avoids discretizing the flat outputs and the constraints that may lead to a great number of constraints. In short, since on its knot vector the basis functions are non-zero and sum to 1, a point on the curve will lie in the convex-hull of the control points. By constraining the control points, we are ensured that the Bspline curve will satisfy the same constraints. However, the distance between the control points and the actual curve is positive but we will show that the conservatism introduced is very reasonable with the considered basis functions. Constraints (13c) can be mapped in the space of the control points. Let us differentiate the B-spline of degree p defined on the clamped knot vector of size n `k `1 such that u i ď u i`1 , i " 0, . . . , n ´k U " p0, . . . , 0 loomoon p`1

, u p`1 , . . . , u k´p´1 , 1, . . . , 1 loomoon

p`1 q (15)
The first derivative can be expressed as a function of the control points P with

s 1 puq " p i"n´1 ÿ i"0 B i`1,p´1 ptq P i`1 ´P i T pu i`p`1 ´ui`1 q (16)
Let us define the vector of new coefficients

Q i " p P i`1 ´P i T pu i`p`1 ´ui`1 q , @i P v0, n ´1w (17)
For the second derivative one has

s 2 puq " i"n´2 ÿ i"0 B i`2,p´2 ptqR i ( 18
)
where R i are the control points of the second derivative. One has

R i " pp ´1q Q i`1 ´Qi T pu i`p`1 ´ui`2 q
, @i P v0, n ´2w (

Now we can express Q and R as functions of P with

Q " A Q P , R " A R Q " A R A Q P (20)
where matrix A Q P R nˆpn`1q and A R P R pn´1qˆn . Similarly, control points of the third derivative are given by

S i " pp ´2q R i`1 ´Ri T pu i`p`1 ´ui`3 q , @i P v0, n ´3w (21)
Now, one can easily set semi-infinite bounds on the derivatives coefficients Q, R and S that are linear in P . All the constraints can be rewritten as functions of the control points P . One wants to solve the following problem on each axis.

Problem 4. Find P such that: min P P T pB T r B r qP s.t. @ P , B j p0q D " η piq 1 , @pi, jq P v0, r ´1w 2 @ P , B j pT c q D " η piq 2 , @pi, jq P v0, r ´1w 2 ´v ď Q i ď v, @i P v0, n ´1w ´ā ď R i ď ā, @i P v0, n ´2w ´j ď S i ď j @i P v0, n ´3w

(22a) (22b) (22c) (22d) (22e) (22f)
where B r is the r-th derivative of the B-spline basis function. We recall that r " 3 for the case of a quadrotor and we minimize the jerk. Problem 4 will be solved using qpOASES [START_REF] Ferreau | qpOASES: A parametric active-set algorithm for quadratic programming[END_REF] that implements an online active set strategy. Note that the connection time T c is fixed and we found that choosing T c " τ generates a reasonable amount of successful connections.

VII. SIMULATION AND EXPERIMENTAL RESULTS

In this section we show some results from different scenarios for the 3D quadrotor. Figure 6 shows two optimal trajectories and Fig. 7 shows the constrained derivatives along a connection considering B-splines of order 4. With the given degree we can see that the curves (e.g., the jerks) are not penalized by conservatism. Finally, Fig. 8 shows the tracking performance for a simulated quadrotor in V-Rep using controller [START_REF] Lee | Geometric tracking control of a quadrotor UAV on SE(3)[END_REF]. We can see that despite considering the thirdintegrator model approximation we can follow the optimal trajectory quite accurately regarding the attitude tracking.

For the experiment illustrated in this section we used a MK-Quadro from MiKroKopter equipped with a front-looking camera with a field of view of 45 ˝tracking the AprilTags with ViSP [START_REF] Marchand | Visp for visual servoing: a generic software platform with a wide class of robot control skills[END_REF]. The setup includs an on-board ODROID-XU4 Linux computer running ROS and the TeleKyb framework [START_REF] Grabe | The TeleKyb Framework for a Modular and Extendible ROS-based Quadrotor Control[END_REF] for controlling the quadrotor. An optimal trajectory computed off-line using a jerk input j " 4m.s ´3 is tracked by the system (see Fig. 9) in presence of two obstacles (blue boxes) and four landmarks. We invite the reader to refer to the attached video for more simulation and experimental results.

Finally, Problem 4 is solved within 5 ms for the 3D quadrotor after about 90 SQP iterations and the planner is able to find an optimal trajectory in 5 to 10 seconds. The presented algorithm has been also applied to the unicycle case by considering constraints on the real system inputs. Details and results are reported in [START_REF] Penin | Contributions to optimal and reactive vision-based trajectory generation for a quadrotor uav[END_REF].

VIII. CONCLUSION AND FUTURE DIRECTIONS

In this work we proposed to incorporate perception constraints in a graph-search planner for planning minimumtime and feasible trajectories for flat dynamic systems. The optimization framework allows exact connection between a given initial and final states while ensuring collision avoidance and bounded final uncertainty at the goal by accounting for the state uncertainty at the planning stage. In this paper (a) An optimal trajectory providing robust collision avoidance and guaranteed visual perception.

(b) In this case the quadrotor is able to increase its height to compensate for the rotation of the camera and to enlarge its field of view. Fig. 6: Two optimal trajectories for the quadrotor with τ " 0.35s and j " 10m.s ´3 in a 12x8x5m 3 operating region considering four visual landmarks (red dots). The initial and final states are chosen such that no landmark is visible so the quadrotor starts with some uncertainty and is able to reach the goal with a bounded uncertainty by observing the landmarks during its motion. Note that the motion primitives are not represented. The quadrotor is tracked with a Vicon system and follows an optimal trajectory (in green) along which landmarks (the AprilTags in orange) are visible on some portions. The lower right figure shows an Apriltag tracked using ViSP when the quadrotor is at the configuration circled in red. The evolution of the uncertainty is shown below after running the Kalman filter on the recorded data and using the AprilTags detection. The landmark at the goal is not taken into account in the planning and is was used to check that the quadrotor reaches its goal within the expected confidence region.

we consider visual measurements that are function of the attitude and propose an efficient optimal graph rewiring by exploiting the convex-hull property of B-splines. Of course, other exteroceptive sensors could be considered. The planner success rate depends on the motion primitives parameters, the ball regions radius and the maximal number of iterations. It could be possible to replan optimal trajectories during motion and even consider dynamic obstacles for the unicycle case (see [START_REF] Penin | Contributions to optimal and reactive vision-based trajectory generation for a quadrotor uav[END_REF]). Note that the derivative-free Kalman filter in the flat space drastically reduces the computation load for the state estimation compared to an EKF with the real nonlinear quadrotor dynamics. Then, we assumed the landmarks position is known but it would possible to incorporate their position uncertainty in the planner. Finally, we believe the tripleintegrator approximation of the quadrotor could be modelled as an additional noise on the model. This would allow a more adequate representation of the real model.

Fig. 7 :

 7 Fig. 7: Constrained velocities, accelerations and jerks along the connection trajectory with bounds v " 2m.s ´1, ā " 4m.s ´2 and j " 10m.s ´3. The small squares represent the B-spline control points.

Fig. 8 :

 8 Fig. 8: Plots of the attitude tracking. The dashed lines are the command values while the solid lines show the actual robot attitude in V-Rep.

Fig. 9 :

 9 Fig.9: Snapshots of the quadrotor during the experiment. The quadrotor is tracked with a Vicon system and follows an optimal trajectory (in green) along which landmarks (the AprilTags in orange) are visible on some portions. The lower right figure shows an Apriltag tracked using ViSP when the quadrotor is at the configuration circled in red. The evolution of the uncertainty is shown below after running the Kalman filter on the recorded data and using the AprilTags detection. The landmark at the goal is not taken into account in the planning and is was used to check that the quadrotor reaches its goal within the expected confidence region.

This work was supported by the French Direction Générale de l'Armement (DGA).