Combinaciones no marcadas

solution if we enlarged the search space.

Problem 5 can be restated as a NLP as follows.

Problem 6 find P , T , such that: min P ,T T s.t. σχptq “ σχt, σχpt ` T q “ σχ˚, β<sub>ipsq P Ω, @s P rt, t ` T s, i “ 1, . . . , N,</sub> u_{psq P U, @s P rt, t ` T s,} (6.5a) (6.5b) (6.5c) (6.5d) (6.5e) where σχt “ φ´1χ pχtq and σχ˚ “ φ´1_χ pχ˚q.

At this point, any general-purpose optimization strategy can be used to find a numerical solution to Problem6. Unfortunately, due to the non trivial non-linearity of (6.5d–6.5e), Problem 6 cannot be proven to be convex. The optimization will thus, in general, return a local minimum.

6.4

Recursive online control

Once Problem6is solved, the resulting flat output trajectory could be used in (2.23) for computing the control inputs u to be fed to the system. In practice, however, different sources of disturbance (e.g. noise, miscalibrations, neglected dynamics, and so on) will make the robot to quickly diverge from the planned trajectory when using such an open-loop control strategy. In order to cope with these uncertainties and disturbances, we then incorporate a feedback action in the considered optimization schemes.

The reasons are multiple: i) since the B-spline order is minimal the snap is piecewise continuous and the inputs can be very sharp (see Fig. 6.3). If one sends such values to the controller we can observe a deviation from the original trajectory. Now, if one increases the B-spline order we can see that the system model integration on smoother inputs result in a more accurate resulting trajectory. ii) having an extra feedback action from the controller provides more robustness to uncertainties and a higher stability.

As already introduced in Sect. 4.2 we take inspiration from Model Predictive Control [189] to perform an on-line re-planning of an optimal trajectory by solving Problem6each time a new visual measurement is available. By doing so, we expect

Figure 6.3 – Example of (bounded) motors thrust profiles considering a parametrization of the flat outputs with a 4-th order B-spline basis.

to improve the system performance while, more importantly, ensuring the satisfac- tion of the visibility constraint (6.5d). Finally, instead of feeding the optimal inputs sequence directly to the system we send the optimal trajectory to the trajectory controller [49] as in [141,1] at the solver rate.

On the one hand, this allow to reject, to some extent, the disturbances acting on the system. On the other hand, however, the optimality of the resulting trajectory can be compromised and, more importantly, the visibility constraints (6.5d) can be violated.

6.4.1 Trajectory re-planning strategy

A major issue in the implementation of receding horizon control is handling the computational delay associated with the real-time optimization. We present here our method for designing an initial guess and take delay into consideration for the re-planning.

The re-planning strategy is best explained by a visual example, shown in Fig.6.4. Let us assume that, in a previous planning step, at time t_{“ t}k_´1, the resolution

of Problem 6 generated a trajectory σk_´1, represented in red in the figure. The

system is now at time t_{“ t}k and a new visual measurement becomes available to

be used in the innovation step1

of the state observer to produce an estimation of the current system state ˆχ_t. This estimation will, in general, be different from the expected system state Φχpσk´1ptqq due to the non-idealities mentioned above. A

new optimal trajectory should, hence, be planned by solving Problem 6 and using the current state estimate to compute the initial condition σχt.

Unfortunately, the resolution of Problem 9 requires a non-negligible time to complete. This time will, in general, vary, depending on the quality of the initial

1

Note that we trigger the planning at camera rate and not at the estimation rate. This is motivated by computational limitations and by the fact that, as already mentioned, the inter- frame estimation obtained by dead reckoning is expected to have a much lower accuracy.

6.4. Recursive online control

Figure 6.4 – Single instance of the re-planning process. The red line represents the trajectory computed in a previous planning iteration. The robot is following this trajectory when, at time tk, a visual measurement and a new state estimation become available (green dot).

The red trajectory is split and clamped to this measurement, resulting in the green line. The first part of this latter is immediately used as reference for the controller. The second part (the dashed green line) is fed as initial guess to the solver of Problem6, and also used to predict the state in which the system will be at time tk` δtp, when the optimization will

be over. Finally, the blue line is the new optimal trajectory resulting from the numerical resolution of Problem 6. The process is repeated again at tk`1, when a new measurement

is available.

guess for the optimization variables, on the number of necessary iterations and on the available computational resources. Here, for simplicity, we assume that the processing will be concluded after, at most, a constant maximum duration δtp,

possibly by introducing a watchdog timer and accepting an intermediate sub-optimal solution.

For computing the system control inputs while the optimization is running, we simply “adapt” the previous trajectory to the new initial conditions by using a fast procedure that does not involve the resolution of Problem 6. First of all, we split the trajectory σk_´1at time tk, as described in Sect.6.4.2, to extract only its second

part σ`<sub>k´1</sub> (the dashed red curve in Fig. 6.4). Then, we look for a new trajectory σ´<sub>k</sub> (represented in green in Fig. 6.4) that is “as close as possible” to σ`_k´1, but starts from Φ´1χ pˆχtq. Details about this step are provided in Sect. 6.4.3. Note that

this “temporary” trajectory σ´_k is sub-optimal and its calculation does not take into account any of the actuation and visibility constraints (6.5d–6.5e), which, as a consequence, could be violated. However, we accept this risk in order to be able to provide an immediate update of the reference trajectory to the new state estimation while a better solution is being computed by appropriately resolving Problem 9 as follows.

During the optimization process, the system will, most probably, move away from the current state χt. As a consequence, if χptq were used as initial condition

in (6.4b) (or, equivalently, (6.5b)), the newly planned trajectory would not start from the actual state of the robot at time t_{` δt}p. We mitigate this problem by

using the trajectory σ´_k also to predict (by a simple B-spline evaluation) the value of the flat outputs corresponding to the state ˆχ_topt in which the system will be when the optimization will be over. This value is used as initial condition in Problem6. Finally, since we use recursive optimization methods to find a solution to Prob- lem6, we also need to provide an initial guess for the optimal trajectory. This initial guess is computed by splitting the trajectory σ´_k at time t_{` δt}p (green dashed line

in Fig.6.4) as described in Sect.6.4.2and taking the second part (green dashed line in Fig.6.4) of the trajectory.

The optimization can finally run and a new optimal trajectory (the blue one in Fig. 6.4) will be generated. Such trajectory will be used to control the system starting from time t_{` δt}p until a new measurement becomes available at time

t_{“ t}k`1. At the arrival of a new measurement the above procedure is repeated.

This strategy allows to re-plan online an optimal trajectory each time a new vi- sual measurement is available. Each one of the generated trajectories could be used directly in (2.23b) to calculate the robot inputs. As already mentioned, however, an alternative possibility is, instead, to use them as references for a fast trajectory tracker. This second possibility is appealing because it allows to fully exploit the sensing capabilities of the robot: between two visual measurements, in fact, an es- timation of the quadrotor state can be obtained, at a much higher frequency, by using the IMU for dead reckoning. A fast trajectory tracker can, thus, use this information to reduce the effect of non-idealities between two planning steps.

Note that, as the quadrotor approaches the desired state, the planning distance and time horizon tend to zero, potentially introducing numerical issues in the res- olution of Problem 6. To overcome this problem, when the system is close to the desired goal, we deactivate the re-planning and directly feed the trajectory tracker with the desired state χ˚_.

6.4.2 B-spline splitting

An advantage of using B-spline trajectories for motion planning is that there exist lightweight and easy algorithms to perform different manipulations on their shape. One such manipulation, that we perform multiple times in the recursive algorithm described in Sect.6.4.1, is the splitting. Details about how to split a B-spline curve at a point and how to calculate the knots and control points of the resulting parts can be found in many sources, such as [190].

6.4. Recursive online control An undesirable effect of the splitting operation is that it also modifies the knot sequence and possibly (depending on the position of the split) even eliminates some knots. In order to maintain a constant number of uniformly distributed knots (and thus a constant number of control points acting “evenly” on the whole spline length), after the split, we perform a sequence of knot insertion and knot removal operations (see [190]) meant to redistribute the knots of the new trajectory evenly.

De Boor’s algorithm is a generalization of de Casteljau’s algorithm. It provides a fast and numerically stable way for finding a point on a B-spline curve given a u in the domain. The core of the algorithm lies in the knot multiplicity rule: if a knot u is inserted m times to a B-spline/NURBS curve, the last generated new control point is the point on the curve that corresponds to u. Meaning that we only need to insert u enough number of times so that u becomes a knot of multiplicity m. If u is already a knot of multiplicity s, then inserting it m_{´ s times would be sufficient.} Indeed, after inserting u m times, the triangular computation scheme yields one point. Because the given B-spline/NURBS curve must pass by this new point, it is the point on the curve corresponding to u. Note that this argument holds even if u is inserted as an existing knot. The depicted procedure is more formalized in Appendix B.2.1. This technique is also applied to evaluate the spline at u. We simply need to insert u m times and the last point is p_puq

Since the given B-spline curve is subdivided at its knots, each curve segment has no internal knots. Moreover, the subdivision process makes the internal knots to have multiplicity m_{` 1, and the curve segment is “clamped” at the first and last} control points of each curve segment.

In the process of subdividing a B-spline curve, a large number of control points will be introduced. Therefore, manipulating a B-spline curve is easier than manip- ulating its component Bézier curves. Moreover, the B-spline curve of degree p is Cp´m continuous at a knot point, where m is the multiplicity of the corresponding knot. When we manipulate a B-spline curve by moving control points, this continu- ity is always maintained. However, if a B-spline curve is subdivided into a sequence of Bézier curves, maintaining the continuity at the joining control points would be a challenging task. Consequently, handling a B-spline curve is much easier than handling a sequence of Bézier curves.

6.4.3 Adapting previous trajectories to new initial conditions In this section we describe how to efficiently “adapt” a previously computed B-spline trajectory (e.g. the trajectory σ`_k´1 represented by a red dashed line in Fig.6.4) to a new estimation of the current robot state (green dot in Fig. 6.4). To perform this operation we exploit two important properties of B-splines:

• The local support property stands that the shape of the curve in a knot span psk, sk`1q is only determined by a subset of k of the B-spline control points.

• The convex hull property guarantees, instead, that in each knot span, the spline curve is locally contained in the convex hull of the same subset of control points. In practice this allows to conclude that changing the first control points (those determining the initial state of the system) will not affect the shape of the spline towards its end (in particular the final system state will not change) and that two splines with similar control points (according to some norm) are also geometrically close to each other.

Given a spline σ`_k´1, with control points P , the control points P´ of the new spline σ´_k can then be computed by solving the following linear quadratic optimiza- tion.

Problem 7 Find a vector of control points P´ such that min P´ 1 2 nr ÿ j“1 › › ›rj´ r ´ j › › › 2 `1₂ nψ ÿ j“1 › › ›ψj´ ψ ´ j › › › 2 s.t. σχptq “ σχt, (6.6) (6.7) Note that Problem7does not take into account the actuation and visibility con- straints in (6.5d–7.9h). While we cannot formally guarantee that these constraints will not be violated, we want to stress that the resulting trajectory is only used for a short amount of time, namely the time needed for the numerical resolution of Problem 6. Introducing a saturation of the control commands one still guarantees the satisfaction of (7.9h) at the cost of introducing a deviation of the robot from its nominal trajectory. Finally, by introducing some security margins in the definition of Ω, one could also reduce the probability of losing feature track in practice.