Variedades afroamericanas de idiomas europeos

We aim at solving an optimal control problem connecting an initial state s0 and a

final state sT in a minimum time T . Let Xf ree“ r

¯v,v¯sˆr¯ω, ¯ωs denote the admissible input space. Let us define the following optimal control problem.

Problem 11 Find the input u_{p.q and time T such that:} min

u_p.q,T T

s.t. s_{p0q “ s}0,

s_{pT q “ s}T,

maxteigpPη_{pT qqu ď ¯λ,}

pvpτq, ωpτqq P Xf ree _{@τ P r0, T s} (8.5a) (8.5b) (8.5c) (8.5d) (8.5e) where eig_pPη_{pT qq P R}2 _{contains the eigenvalues of the position covariance matrix}

at the goal state sT. The desired bound on the position uncertainty is defined by

¯

λ ą 0. For nonlinear systems such as a unicycle (and 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 work, we stick to the flat space in order to perform a 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 [209]. Finally, we impose input constraints with (8.5e). Considering the linear equivalent system one defines the process model. When a landmark is visible we have

9s“ As ` Bu ` ζ, y_{“ Cs ` ν}

(8.6a) (8.6b) where ζ _{P R}4 is the process noise and ν _{P R}2 is the measurement noise. Assum- ing the velocity is estimated through filtering of position measurements, the above matrices 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 ¸ (8.7)

8.4. Problem formulation In the next sections we show how Problem 11 can be transformed from an infinite dimensional optimal control problem to a finite dimensional graph-search problem. We choose to extend two graphs to increase the chance and the rate of convergence to a solution, especially in complex and cluttered environments. Moreover, it generally propagates fewer vertices than with a single graph [210].

8.4.1 Motion primitives

As in [145] we use polynomials to parametrize the flat state components and generate motion primitives to explore the flat space in a discrete way. More precisely, by applying a number of sampled constant acceleration inputs (see (8.3)) along each axis uk P UM :“ r´umax, . . . , umaxs

2

for a duration τ ą 0 one can iteratively build a graph G_{pV, Eq rooted in state s}0, where V is the set of discrete states denoted as

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. 8.1). A motion primitive represents the state s_{ptq starting at a state s}0 on tP r0, τs with a curve defined as

s_{ptq “ Mpu}m, s0, tq :“ « umt 2 2 ` 9η0t` η0 umt` 9η0 ff (8.8) These trajectories reflect the system dynamics thanks to differential flatness and provide the minimum acceleration between the states s0 and spτq [33]. The free

flat space will be explored with a propagation of these motion primitives further detailed in Sect. 8.5.1. Naturally, changing the input bounds and duration τ will affect the free space coverage.

Problem 11 can be reformulated as Problem 12 in the graph representation where we seek the trajectory connecting the initial and goal states with the optimal control sequence u˚

k and the minimal number N˚ of motion primitives.

Problem 12 Find the sequence uk and N such that:

min uk,N N s.t. s0 “ sinit, sN “ sgoal, maxteigpPηNqu ď ¯λ, z_puk, si, tq P Xf ree @i P v0, Nw (8.9a) (8.9b) (8.9c) (8.9d) (8.9e) where Pη_N is the covariance matrix on the position at the goal vertex, function z : puk, si, tq ÞÑ rvptq, ωptqsT computes the system inputs on discretized states sampled

have a total time N˚_{τ. Finally, collisions are avoided by considering the robot’s}

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 and inputs constraints are not added to the graph.

The advantage of graph-search planners in contrast to optimization-based meth- ods (and especially gradient-based) is that complex constraints are not directly part of the optimization problem but are checked at each vertex expansion. Moreover, optimization-based methods may not be adapted to problems involving discontin- uous constraints gradients as for (8.9d) that is the solution of a stochastic process with intermittent Kalman updates and possibly large periods without any sensing information. Evaluating such a gradient for gradient-descent solvers would be chal- lenging and computationally intense since it is also completely re-evaluated at each iteration. In the next section we show how state uncertainty is included in visual perception and in collision avoidance to guarantee perception of visual measure- ments and safe navigation to a given level of confidence.

8.4.2 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?λ 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 circle (a sphere in 3D) with radius σ99% as representative of the robot occupancy. It will vary with

the pose uncertainty and will be incorporated in the planner for ensuring robust collision-free paths.

Now, let us include the position uncertainty in the visual measurements. A visual landmark at known position ηL “ pxL, yLq is visible when its bearing angle

φ is smaller than the field of view angle 2α and lies in a given range rc from the

camera (see Fig.8.3). One has

φ_{“ Arctan2}ˆ y ´ yL x_{´ x}L ˙ ´ θ, rc “ }η ´ ηL} (8.10a) (8.10b) The uncertainty ∆φP R related to the bearing angle φ can be obtained as a function of the state uncertainty and is given by

∆φ“ˆ Bφ Bη Bφ B 9η ˙T˜ Pη 0 0 P_9η ¸ ˆ Bφ Bη Bφ B 9η ˙ (8.11) where Pη and P9η denote the position covariance matrix and the linear velocity co-

8.5. Building the graph