A Sensitivity-Aware Motion Planner (SAMP) to Generate Intrinsically-Robust Trajectories

Abstract

Closed-loop state sensitivity [1], [2] is a recently introduced notion that can be used to quantify deviations of the closed-loop trajectory of a robot/controller pair against variations of uncertain parameters in the robot model. While local optimization techniques are used in [1], [2] to generate reference trajectories minimizing a sensitivity-based cost, no global planning algorithm considering this metric to compute collision-free motions robust to parametric uncertainties has yet been proposed. The contribution of this paper is to propose a global control-aware motion planner for optimizing a state sensitivity metric and producing collision-free reference motions that are robust against parametric uncertainties for a large class of complex dynamical systems. Given the prohibitively high computational cost of directly minimizing the state sensitivity using asymptotically optimal sampling-based tree planners, the proposed RRT*-based SAMP planner uses an appropriate steering method to first compute a (near) time-optimal and kinodynamically feasible trajectory that is then locally deformed to improve robustness and decrease its sensitivity to uncertainties. The evaluation performed on planar/full-3D quadrotor UAV models shows that the SAMP method produces low sensitivity robust solutions with a much higher performance than a planner directly optimizing the sensitivity.

Controller and Trajectory Optimization for a Quadrotor UAV with Parametric Uncertainty

“…We now report the results of a statistical analysis for the three controllers of Sect. III when considering the three optimization problems (11)(12)(13). For the analysis we generate N traj = 25 initial trajectories s d (a, t) starting at the origin and coping with the initial/final state constraints and input saturations as in (11)(12)(13).…”

“…III when considering the three optimization problems (11)(12)(13). For the analysis we generate N traj = 25 initial trajectories s d (a, t) starting at the origin and coping with the initial/final state constraints and input saturations as in (11)(12)(13). These initial trajectories are restto-rest motions (from a hovering state to a hovering state) with a final position randomly generated inside a spherical shell of 4 to 6 m centered at the origin, and a final yaw angle randomly generated in the interval [−π/2, π/2].…”

“…For the sake of exposition, in the following we will let IN IT represent an initial (non-optimized) trajectory, OP T a the corresponding optimized trajectory solution of ( 11), OP T k the corresponding optimized trajectory solution of ( 12) and OP T ak the corresponding optimized trajectory solution of (13). We thus obtain a total of 4N traj trajectories: starting from the origin and ending at the same target location, with the parameters p being randomly drawn from (6).…”

“…Of course, different choices are also possible such as minimization of (the integral) of the sensitivity norm along the whole trajectory (for increasing the average tracking accuracy during motion). Note also that additional constraints, such as obstacle avoidance, could be easily added to the optimization problem by exploiting the tubes on the states, see [13] for an example in this sense. One can also optimize the sensitivity at specific waypoints along the trajectory where, for instance, the effects of uncertainties in high speed regimes can be minimized.…”

“…where the the allocation matrix T from ( 15) is evaluated at the nominal parameters p c . The vector of control gains k c considered in problem (12)(13) for this controller consists of all the 15 gains in (16), that is, the three gains (k p• , k d• , k i• ) for the x, y and z channels, and the two gains (k p• , k d• ) for the ϕ, θ and ψ channels.…”